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I. INTRODUCTION 


A. MOTIVATION 


Computer systems have experienced a dramatic increase in performance and power in 
a relatively short period of time. This increased performance makes more realistic and 
immersive computer simulations possible for research, training and entertainment 
purposes. However, as rapidly as computer systems have increased in capability, one 
specific area of computer simulations lags behind. Research into more realistic and natural 
interactive input devices has come along very slowly and has not had the same success as 
other aspects of computer hardware. The need for intuitive interaction in virtual 
environments (VE) has driven the development of several different approaches to 
accomplishing total immersion into synthetic worlds. The main thrust of research in this 
realm has been in the area of producing new and improved sensors for tracking the human 
form and other objects effectively and efficiently and with as little hindrance to the user as 
possible. 

In recent years, inertial sensing technology has become a viable alternative to systems 
based upon mechanical, electromagnetic, acoustic, and optical tracking sensors. Inertial 
trackers solve the problems encountered when using these systems, namely, shadowing, 
metallic, electronic and acoustic interference as well as limitations in range. The self- 
contained inertial sensors are impervious to any outside interference and function solely by 
sensing the earth's magnetic and gravitational fields. A combination of accelerometers, 
angular rate sensors, and magnetometers provide the system with the data required to 


accurately specify spatial orientations. However, inherent limitations and errors associated 


with accelerometers and angular rate sensors necessitates proper filtering be applied in 


order to extract reliable orientation information. 


B. GOALS 


A quaternion attitude filter has been proposed which overcomes the singularities 
encountered when using Euler angles to represent object orientations [MCGH96A]. The 
goal of this thesis 1s to implement a simulation of an inertial system that will provide a 
useful and decisive demonstration of the filter' s applicability to body tracking applications. 
Allegro Common Lisp, ver. 3.0.1 for Windows was chosen as the programming language 
with which to implement the simulation. 

This thesis also takes an existing inertial sensor, the Angularis system, built by 
InterSense, Inc., [INTE97], and presents a quantitative and qualitative analysis of its 
performance. Even though this particular sensor was built specifically for the tracking of 
the human head using a head mounted display (HMD), it is the goal of this study to test the 
applicability of this particular sensor to other types of tracking. Specifically, to determine 
the viability of using such a sensor in human limb segment tracking applications. The 
sensor is evaluated against a system that is very well known and understood by researchers 
at the Naval Postgraduate School (NPS), the Shallow-Water AUV Navigation System 
(SANS) which is currently being used in the ongoing Autonomous Underwater Vehicle 
(AUV) research [BACH96A, MCGH95, MCGH96B]. It is hoped that the Angularis 
system can perform as well as or better than the much larger SANS system. This advanced, 
miniaturized inertial sensing technology, used in conjunction with the quaternion filter 


presented in this thesis, may allow for the creation of a full body suit which would be 


capable of tracking an entire human body within VE applications much more reliably and 


accurately than current systems. 


C. ORGANIZATION 


Chapter II of this thesis surveys existing sensor technology and presents current work 
in the area of human body tracking. Chapter III presents a detailed mathematical 
formulation of a quaternion filter. The quaternion filter is presented as an alternative to the 
use of Euler angles in representing object orientations. The quaternion representation 
avoids singularities experienced when tracking objects through the vertical axis as happens 
when using the more familiar Euler angles. Chapter IV elaborates on the use of the 
quaternion filter by presenting a simulation model, created as part of this research, to show 
the viability of using such a filter in conjunction with inertial sensors for object tracking in 
VE's. Chapter V presents the results of quantitative and qualitative evaluations of the 
Angularis inertial tracker. Comparisons are made between the Angularis tracker and the 
SANS system, currently being researched in the NPS AUV project. Chapter VI, the last 
chapter, presents conclusions about the results of this research and some recommendations 


for possible future work with quaternion filters and inertial trackers. 
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Il. BACKGROUND 


High-performance computer graphics are being applied to an expanding range of 
domains [BADL93]. One of the most dramatic and exciting areas is the real-time, 
interactive representation of the human form in training, research and entertainment 
applications. Several requirements must be met in order to effectively create a realistic 
representation, namely: 1) create a human model with the desired level of detail, 2) define 
the level of control and user inputs for manipulating the model, and 3) provide inputs to the 
model in a timely fashion in order to achieve real-time performance [SKOP96]. Task 
number two, user input methods, is the focus of this thesis. | 

The primary purpose of any tracking device is to provide an intuitive interface between 
human and machine in order to achieve the desired illusion of total immersion. The user 
must be allowed to interact with the VE in a familiar and natural way. Therefore, the use 
of standard 2D pointing devices is unacceptable if the goal is to achieve realistic 
interaction. To that end, a few different types of trackers have been introduced which 
employ varying methods to capture the position and orientation data of a tracked object 
[FREY96A]. The remainder of this chapter presents four specific types of trackers as well 


as quantitative measures by which they can be compared and evaluated. 


A. MEASURING MOTION TRACKER PERFORMANCE 
Although several different sensor tracking technologies have been developed and 
applied to VE applications, there exists no standard evaluation method by which to obtain 


quantifiable comparisons between different types of trackers [SKOP96]. Typically, 


financial constraints and the intended application will dictate which tracker is most 
appropriate for a given project. [MEYE92] suggests some key measures by which tracking 
systems may be evaluated, namely, (1) resolution and accuracy, (2) responsiveness, (3) 
robustness, (4) registration, and (5) sociability. Resolution can be defined as the smallest 
change which can be detected by a given tracking system. The level of detail required by 
the application will define the resolution required. Higher resolutions are necessary for 
applications which necessitate the tracking of small, precise movements. Accuracy is the 
sensor’s error range. Given some orientation or position information, the accuracy will 
determine the range for which the raw data is correct. For example, inertial systems use 
angular rate sensors which tend to drift over time due to inherent bias errors. The bias 
determines the accuracy of the sensor and must be accounted ir by filtering techniques. A 
system’s sampling rate, data rate, update rate, and lag (or latency) all combine to describe 
the overall responsiveness. Sampling rate is simply how often the sensor is checked for 
new data. The rate at which new data becomes available is the system’s data rate. Data 
rate is defined as the number of computed data points per second that the system can 
provide. Most systems will implement a much higher sampling rate than data rate in order 
to assure that new data is not missed. The rate at which the system can provide updated 
position and orientation data to the host computer is the update rate. This data is raw data 
and contains errors. Thus, the information must be filtered before it can be used with any 
degree of reliability. Even then, the accuracy of the position and orientation updates is only 
as good as the filter. Filtering the sensor data also takes time and will hinder real time 


updates. A system’s lag is sometimes referred to as its latency and is perhaps one of the 


most important specifications of a tracking system. Latency is the measure of the delay 
between the movement of a tracked object and the corresponding movement of the 
computer representation of that object in the VE. High latency in a tracking system is 
undesirable. When sufficiently large, it produces visually disturbing anomalies in the 
computer simulation. These anomalies tend to disorient and confuse participants, possibly 
even inducing nausea and vomiting [FOXL94]. Robustness is the measure of a system's 
susceptibility to noise and other interference from outside sources. Types of interference 
include shadowing, metallic, electronic and acoustic. Registration is defined as the 
coherence between the sensor's actual position and orientation and reported position and 
orientation. Finally, sociability describes a system's maximum range of operation, its 
working volume, and the ability to track multiple targets within that operating range. 
Working volume is that volume in which the tracker can accurately report position and/or 
orientation information. [MEYE92] 

These measures provide a means by which researchers and developers can 
quantitatively determine the best technological alternative for a given application. 
However, factors such as availability, cost, and ease of use must also be taken into 


consideration before making a final decision [SKOP96]. 


B.. TYPES OF MOTION TRACKERS 


dE Mechanical 


There are generally two different types of mechanical trackers, body-based and 


ground-based [FREY96B]. Body-based (exo-skeletal) systems are characterized by 


interconnected mechanical linkages mounted on the user's body which measure joint 
angles directly. An example is shown in Figure 1. Since no external source is required, 
these sensors are not susceptible to external interference and are very accurate. The 
physical linkages are well suited for providing haptic responses. Haptic responses are 
force feedback cues that enable the user to experience simulated exertion forces during a 
VE simulation, further enhancing the realism of the environment and immersion of the 


USET. 





Figure 1: IPORT Soldier Station [NRG97]. 


The same characteristics, however, that make mechanical tracking devices good for 
haptic feedback also make such systems cumbersome, heavy, and not very comfortable to 
use for extended periods of time [FREY96A]. Also, the manner in which the harness is 
attached to the body must be considered carefully. In order to obtain reliable joint angle 


data, relative motion between the physical linkages and the human body must be 


eliminated. The joints of the device have to remain aligned with the joints of the human 
user in order to ensure the co-location of their respective centers of rotation [FREY96B]. 
Ground-based systems are normally not attached to the user. Rather, they track the 
position and orientation of a separate implement which is manipulated by the user 
[FREY96B]. These systems, like their body-based counterparts, are very accurate and do 
not suffer from external interference. However, their applicability is limited by their 
restrictive working volume. These devices are usually not portable and require a 


designated area for their use (Figure 2). 





Figure 2: Graphics Force Feedback System [HANC96]. 


In general, mechanical trackers are very precise and responsive to user inputs and are 
not hindered by external interference. Applications requiring a limited range of motion and 


where user immobility is not a problem are best suited for this type of sensor. [MEYE92] 


2. | Electromagnetic 


Electromagnetic trackers utilize artificially-generated electromagnetic fields to track 
position and orientation. A fixed transmitter generates three orthogonal electromagnetic 
fields which induce voltages in three orthogonal coils located in each detector attached to 
the tracked object [FREY96B]. These induced voltages are related to the spatial orientation 
of the detector relative to the transmitter [FREY96B]. This type of system is typically 
referred to as a sourced system with the source being the transmitter which generates the 
reference electromagnetic fields. Currently, there are two implementations of 
electromagnetic trackers available, alternating current (ac) and direct current (dc) 
[MEYE92]. 

The two implementations work in the same manner except for the way that the 
reference magnetic field is emitted. The source in an ac System emits continuously 
changing magnetic fields producing circulating (eddy) currents which in turn produce 
secondary ac magnetic fields that distort the emitter field pattern [MEYE92]. The dc 
implementation, on the other hand, emits a sequence of pulses which reduce the effect of 
distorting currents [MEYE92]. Since eddy currents are created only when the magnetic 
field is changing, dc systems only generate distortions at the beginning of a measurement 
cycle [MEYE92]. When the field reaches its steady state, no eddy currents are created, 
reducing overall system distortion [MEYE972]. 

Electromagnetic tracking devices are relatively inexpensive and can be used to track 
numerous objects position and orientation with accuracies adequate for some applications 


[FREY96B, SKOP96]. The disadvantage, however, is that they have a limited working 
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volume due to the fact that they are sourced and the magnetic field strength diminishes with 


distance [MEYE92, SKOP96, POLH93]. 


3. Acoustic 

High frequency ultrasonic sound waves are used to track objects by either the 
triangulation of several receivers (time-of-flight method, TOF) or by measuring a signal's 
phase difference between transmitter and receiver (phase-coherence method, PC) 
[FREY96B, LIPM90]. The TOF method uses the calculated speed of sound through the air 
to determine the distance between several transmitters and one receiver or vice-versa 
[SKOP96]. Triangulation formulae are then used, with the calculated distances, to 
determine object position [LIPM90]. Tracking can be extended to 6-DOF by placing 
sensors at three separate locations on the same object. Systems utilizing the PC method 
measure the phase difference between transmitted and received signals and determine the 
corresponding change in position. However, when an object moves farther than one-half 
wavelength in a single update period, tracking errors will occur in the position calculation 
[SKOP96]. Consequently, over time, small errors in position determination will result in 
large errors overall [FREY96B]. 

Acoustic systems based on TOF are susceptible to ranging errors due to reduced data 
rates at greater operating distances. TOF systems are also more vulnerable to spurious 
noise sources at any range. PC systems are less vulnerable to noise and in general 
experience improved accuracy, responsiveness, range, and robustness because of their 


higher data rates, but PC systems are prone to cumulative errors over time. Both systems 


11 


must maintain line-of-sight between transmitters and receivers in order to avoid position 


data errors which result from sensor occlusion (shadowing). [MEYE92] 


4. Inertial 

Inertial tracking systems use a combination of linear acceleration, angular rate and 
magnetometer sensors to determine rigid body orientation. Typically, angular orientation 
is determined by integrating the output from the angular rate sensors [FREY96A]. This is 
analogous to integrating linear velocity to find position. However, angular rate sensor 
output is degenerated by an error called drift. Driftis defined as the tendency of bias errors, 
inherent to the sensor, to cause increasing orientation measurement errors over time 
[FREY96B]. The amount of drift error present in an angular rate sensors output is 
dependent upon the quality of the sensor, with higher quality sensors having lower bias 
errors [FREY96B]. This fundamental limitation makes angular rate sensors a short term 
solution to determining a rigid body's spatial orientation. 

In order to compensate for the long term errors introduced by the use of angular rate 
sensors, inertial systems utilize linear acceleration sensors called accelerometers. 
Accelerometers measure the gravity vector, in reference to the local coordinate system, as 
well as forced linear accelerations of the attached rigid body. This confounding of gravity 


measurement is sometimes referred to as slosh [FOXL94]. Therefore, if the forced 
acceleration is à , and the acceleration due to gravity is 2 , then the acceleration measured 


by the accelerometer is ümeasured — à- 2 [FOXL94]. Since most real objects do not 


continuously accelerate, the average of the forced linear acceleration vector will eventually 
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become zero [FREY96B]. As the average of the forced acceleration vector approaches 
zero, the averaged accelerometer output will therefore approach ämeasured = 3 
When averaged over the long term, the accelerometer will produce the gravity vector, 
expressed in body coordinates, which in turn can be used to calculate the rigid body’s pitch 
and roll angles relative to earth-coordinates [FREY96B]. This observation justifies the use 
of a complementary filter [BROW92] which incorporates the short term accuracy of the 
angular rate sensors with the long term stability of the accelerometers to provide accurate 
orientation information for a tracked object. 

The third component of inertial systems is the magnetometer. The magnetometer 15 
sensitive to the earth’s magnetic field and can sense rotations about the local vertical axis 
[FREY96B]. Since accelerometers cannot detect rotations about the local vertical, 
magnetometers must be used to correct drift errors in the azimuth calculations made from 
angular rate sensor data. Thus, a careful combination of the data from all three sensors can 
provide a good representation of spatial orientation. A summary of all the sensors 


presented is given in Table 1. 


APA mm [em 


Good in small work- 
ing volumes. Accu- 
racy tends to diminish 
as emitter-sensor dis- 
tance increases. Accu- 
racy adversely 
affected by ferromag- 
netic objects in work- 
ing volume. 














Resolution 






Table 1: Evaluation of Position-Tracking Technologies [MEYE92, SKOP96]. 
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umma quem Temm መ 7 


Responsive- | Good. 
ness 


Robustness 


Good. Not 
sensitive to 
errors intro- 
duced from 
the environ- 
ment. 







Relatively low data 
rates. Filtering required 
for distortions in emit- 
ted field can introduce 


lag. 


Ferromagnetic objects 
create eddy currents 
that distort the emitted 
field causing ranging 
EITOTS. 








TOF: Good at close 
range. Data rates 
diminish as range 
increases. 

PC: Unaffected by 
range. 


TOF: Low data rates 
cause vulnerability to 
ranging errors. 
Robustness dimin- 
ishes as range 
increases and data 
rates drop. 

PC: High data rates 
unaffected by range. 
Excellent robustness. 























Excellent. 

























































Sociability Limited Most effective for TOF: Accuracy and Excellent. 
range. Two small working vol- responsiveness dimin- 
systems can- umes. Distortions ish as range 
not effectively | from induced eddy cur- | increases. Small effec- 
occupy the rents increase with tive working volume 
same working | field strength. can limit sociability. 
volume. Configurations avail- PC: Large working 
able for allowing sen- volume offers good 
sors to share emitters sociability. Increased 
or formultiple emitters | range does not affect 
in same work space. responsiveness. 
Acoustic systems are 
vulnerable to occlu- 
sion. 
Comments Cumbersome. | Available off-the- Acoustic systems are | Expensive and not 





Well suited to 
force feed- 
back. 








shelf. Relatively inex- 


pensive. Most com- 
monly used in current 
VR research. 


starüng to appear in 
marketplace and are 


relatively inexpensive. 


widely available. 





Table 1: Evaluation of Position- Tracking Technologies [MEYE92, SKOP96].(contd.) 
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C. RELATED WORK 


1l. Responsive Workbench ™ 


The Responsive Workbench™ goal is to seamlessly integrate the computer into the 
user’s world [GMD97]. This approach is contrary to the typical VE where the goal is to 
immerse the user into the computer’s world and provide him/her with a virtual presence in 
that world. With the approach taken by the creators of this system, it is possible for 
everyday objects and activities to become inputs and outputs to the environment [GMD97]. 
The display, for instance, is not presented on a traditional computer monitor or television 
screen but on a real 3D workbench (Figure 3). By doing this, the display becomes part of 
the human’s working environment [GMD97]. Computer-generated stereoscopic images 
are projected onto a tabletop via a projector-and-mirrors system as illustrated in Figure 4. 
The 3D effect is observed by wearing shutter glasses. Although currently only one user 1s 
tracked, the system allows other participants to observe the 3D interaction with their own 
shutter glasses. The system also uses a 6-DOF tracking system to track the user's head as 
well as tracking the user’s hands and an input stylus for environment interaction [GMD97]. 

The creators of the Responsive Workbench™ refer to their system as a "Responsive 
Environment", which integrates tracking systems, cameras, projectors and microphones, 
creating a more realistic training and learning environment and challenging traditional 
expectations of what a computer workstation should be [GMD97]. Current applications of 
this technology include: medical training, surgical planning, fluid dynamics visualization 
in a virtual windtunnel, 3D architectural designs, molecular modeling and 3D 


manipulations of molecular models [GMD97]. 
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Figure 4: Projector-Mirror System [GMD97]. 


16 


2. Computer Graphics System with Force Feedback 


A more traditional effort, using mechanical sensors, is shown in Figure 2 [HANC96]. 
This system incorporates both stereoscopic viewing and direct object interaction with a 
force feedback I/O handheld device [HANC96]. The system allows the user to not only 
interact with virtual objects within the VE, but to also “feel” them. [HANC96] does not 
refer to his system as “Virtual Reality” but instead as "Interactive Graphics". He believes 
that bimodal displays are the next step in computer graphics and that these systems are not 
just for virtual reality games, but a serious tool that developers and researchers can utilize 
in their work. 

This system was implemented using two HP 735 UNIX workstations, a Phantom force 
feedback device, and Crystal Eyes for stereoscopic viewing of the computer monitor 
[HANC96]. The research focused primarily on the development and implementation of 
real-time collision detection algorithms which could provide satisfactory user response. 
The result was a system which could generate stereoscopic rendering rates of 20 Hz, force 


update rates of over 1000 Hz with an overall system latency of 50 milliseconds [HANC96]. 


3.  RF/Inertial Head-Tracker 

A new 3D RF positioning system has been developed at Advanced Position Systems, 
Inc, (ASPI Technology). Through the use of a new and innovative “dynamic calibration 
method” developed by Dr. Jun Feng, the 3D RF positioning system improves the 
positioning accuracy down to the millimeter scale [FENG97]. The success of the initial 
positioning system prototype has prompted a proposal for a new 6-DOF RF/Inertial 


tracking system. By incorporating the InterSense 3-DOF inertial tracker [INTE97] for 
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orientation information, itis proposed that a long range, lightweight 6-DOF cordless head- 


tracker utilizing RF technology for position information can be developed [FENG97]. 


D. SUMMARY 


This chapter presents a brief overview of available body tracking technology. Table 1 
gives a consolidated evaluation summary of the sensors presented with respect to five 
performance measures. This chapter does not provide a complete review of all available 
tracking technologies, but rather attempts to present a general overview of this field of 
technology. Interested readers are directed to [FREY96A, FREY96B, INTE97, MEYE92, 
POLH93] for further discussion on these and various other systems which are currently 
available and were not presented here. The next chapter introduces the quaternion filter 
proposed by [MCGH96A] and presents the complete mathematical formulation of this 


approach. 
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Ill. A QUATERNION ATTITUDE FILTER 


In order to completely describe the state of a tracked object, both position and spatial 
Orientation information must be obtained. In robotics, a coordinate frame is used to 
describe the position and orientation of objects with respect to some universal (earth-based) 
coordinate system, Figure 5 provides an illustration of this idea. The coordinate frame is a 
local (body) coordinate system which is rigidly attached to an object in a known way 
[CRAI89]. Calculating the position and orientation of the frame determines the 
“kinematic” state of the object. Various methods exist for representing the rotations 
required to bring coordinate systems into alignment. This chapter presents a comparison 
between the familiar and widely used Euler angle representation for rotations, and the less 
common quaternion method. Also presented is a mathematical derivation of a quaternion 
filter, proposed by [MCGH96A] as an alternative to filters based on Euler angles which 


experience singularities at certain orientations. 





Figure 5: Coordinate Systems (Frames) [CRAIS9]. 
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A. QUATERNIONS VS. EULER ANGLES 


1. Euler Angles 


Orientation information, obtained from the types of motion trackers presented in the 
previous chapter, is given in local frame coordinates and must be translated into universal 
coordinates before the object can be graphically represented within a VE. Translating from 
one coordinate system into another requires the calculation of the rotation(s) and 
translation(s) which bring both systems into alignment. One of the most popular and 
intuitive methods of representing the rotation part of such transformations is by the use of 
Euler angles. The Euler angle method represents the orientation of an object, with respect 
to a known earth coordinate system, by applying three successive rotations, shown in 


Figure 6. 


+. Y 
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“on git azimuth y 






y 


Figure 6: Azimuth, Elevation, and Roll Rotations. 


These rotations about the x, y, and z axes are represented by the “elementary” rotation 


matrices for roll, elevation, and azimuth, respectively, given by [CRAI89]: 
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1 0 0 
R (6) = [0 ር05ፅ -58ቀ (eq. 3.1) 
0 sind cosd 


cos8 0 sin® 
0 1 0 (eq. 3.2) 
—sinO O cos 


R. (3) 


cosy —siny 0 
siny cosy 0 (eq. 3.3) 
0 0 1 


R (Y) 


A significant disadvantage of Euler angles occurs when they are used to estimate and 
calculate orientation angles for tracked objects. A specific example is the navigation filter 
used in the NPS AUV project [MCGH95, BACH96A]. The filter incorporates inputs from 
an onboard Inertial Measuring Unit (IMU), heading, and water-speed sensors with 
intermittent GPS fixes to accurately provide continuous real-time navigational data 
[BACH96A]. The problem occurs when elevation angles of +90° are encountered. Given 
the fact that submarines can go vertical only once, this singularity is not important in such 
navigation problems. However, when applied to human body tracking, singularities can 
occur because there is nothing to prevent many body segments from passing through a 
vertical orientation. In a recent research effort, Euler angles were used by (SKOP96] to 
represent orientations in an implementation of a human body tracking system, using 
Polhemus™ 3-Space® electro-magnetic tracking sensors [POLH93]. The simulation 


Clearly demonstrated the effects of these singularities when limbs moved through the 
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vertical axis. [SKOP96] was forced to employ error-checking programming techniques in 
the software to avoid the problematic orientations and the system crashes that they would 
have caused when encountering angle singularities. 

The reason why elevation angles of +90” are so critical is because Euler angle 
orientation estimations result in divide-by-zero errors at these angles. A schematic 
representation of the attitude estimation part of the navigation filter, developed for tie NPS 
AUV project, shown in Figure 7, illustrates this limitation [MCGH95, BACH96A]. As can 


be seen, the accelerometer estimate of the roll angle, ቀ,- is determined by using the 


corresponding value for the elevation angle, 9; ‚in the following equations 


E 
0 = asin— (eq. 3.4) 
ቀ; ፦ ME 6080, (eg: : ) 


Clearly, when 9, becomes X90? , (eq. 3.5) will be undefined due to a divide-by-zero error 


caused by the fact that cos90° 2 0 . 


In the same portion of the filter, Euler rates are calculated from body rates using inputs 


from the angular rate sensors. The angular rates, in body coordinates, are given by the 


angular rate sensor inputs, (p,q,r) , which are multiplied by the transformation T-matrix 


and converted into Euler rates as defined below 
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6 1 sinbtan® cosbtan$| |p 
ó| * |O cos  -sin$ ||q (eq. 3.6) 
V 0 sinósecO0 cososecO| | r 


Multiplying out (eq. 3.6) yields: 


ዕ = p- qsinótanO + rcosotané (eq. 3.7) 
8 = gcos6-rsind (eq. 3.8) 
Y = qsinosecO 4 rcos$secO (eq. 3.9) 


Again, elevation angles of +90° create divide-by-zero errors in (eq. 3.7) and (eq. 3.9). In 


order to avoid such singularities, an alternative representation is needed. 
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Figure 7: Euler Angle Estimation Portion of SANS Filter 
[MCGH95, BACH96A]. 
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2. Quaternions 


Quaternions are an extension of the familiar complex numbers. Instead of just having 


one imaginary part, represented by i, quaternions have three “imaginary” parts, represented 


by i, j, and k, all having the same value, N-1 . Thus; 


i*i-i?^--1 (eq. 3.10) 
ejejósal (eq. 3.11) 
k *k=k?*=-1 (eq. 3.12) 


Quaternions can also be represented in three different notations. Depending upon the 
particular application, it may be convenient to represent quaternions as either a linear 
combination of four components, a four dimensional vector NOE by the coefficients 
of the linear combination, or as a scalar and a vector. That is, the following three notations 


are equivalent: 


q = w+xi+yj+zk (eq. 3.13) 
9 = (ዝ ፳ 7 2) (eq. 3.14) 
q = (w,v) (eq. 3.15) 


When rotating vectors with quaternions, it will generally be required that unit quaternions 
be used [COOK92]. This is done because of the convenient way in which the inverse of a 
unit quaternion can be calculated. Specifically, the inverse of a unit quaternion is defined 
as the conjugate of the quaternion; that is 


q! =q* = (w, -v) (eq. 3.16) 


where q’! is the inverse of a unit quaternion and q* is the conjugate of quaternion q. 


Any scalar or three dimensional vector can be represented as a quaternion. For scalars, 
the vector (0 0 0) is appended to the scalar w to obtain 
q = (w000) (eque. 19 
In the case of a three dimensional vector, the scalar 0 is appended to the front of the vector 
to get the equivalent quaternion. 
a UE y z) (eq. 3.18) 
Once scalars and vectors have been properly converted to quaternions, quaternion algebra 
can be applied to rotate vectors, multiply scalars, etc. Specifically, rotation of a vector, p, 


by a quaternion, q, is defined as [PAUL9O] 


== 


Dred “ሠና (eq. 3.19) 


where q is a unit quaternion given by 


_ 0 . © 
Jg (cos, usin (eq. 3.20) 
The symbol u in (eq. 3.20) represents the unit vector about which the vector p is to be 


rotated through an angle 0. It should be noted, unlike Euler angles, quaternion rotations 
require only two trigonometric functions to rotate a vector and experience no singularities 


at any angle of rotation. 


B. DERIVATION OF QUATERNION FILTER 


Filters are used to minimize errors and return accurate results in an environment where 
noise corrupts the measurement of the desired data. In the case under consideration, the 


data is the orientation measurements made by inertial sensors used to track human motion 
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within a VE. Inertial sensors are inherently noisy and experience measurement errors due 
to drift, slosh, and various manufacturing imperfections [FOXL94, FREY96A]. This 
section presents a mathematical derivation of the quaternion filter proposed by 
[MCGH96A] and depicted in Figure 8. This filter was developed as an alternative and 
improvement to the Euler angle based filter that was designed for the AUV project at NPS 
[MCGH95, BACH96A]. 

The quaternion attitude filter takes inputs from three separate sensors which, together, 
make up the inertial tracking system. The system consists of accelerometers, angular rate 
sensors, and a 3-axis magnetometer. Accelerometers measure the combination of forced 


linear accelerations and the reaction force due to gravity, measured = 4-2 . Since 


most real-life objects do not experience constant linear accelerations (i.e. a — 0 ), when 


averaged over time, accelerometers on the average return the gravity vector or local 


vertical, Ameasured = zx [FOXL94, FREY96B]. Angular rate sensors measure the 
angular velocity in three axes. Output from the angular rate sensors can be integrated to 
determine position, but because they experience drift errors over time, a combination of 
accelerometers and magnetometers are required to correct the measured data. 
Magnetometers measure the earth’s magnetic field in body coordinates. The main purpose 
of the magnetometer triad is to sense the drift error of the angular rate sensors about the 
vertical axis which can not be sensed by the accelerometers. Together, the three sensor 


types provide an accurate means of calculating orientation measurements for any object. 
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Filter [BACH96B, MCGH96A]. 
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Figure 8 


The quaternion filter computation begins with the normalized input measurements 


from the accelerometers and magnetometer, as shown in Figure 9. 


Magnetometer 
(b, b. b 


2 93) 







Accelerometer 


b b, by 
($ 2)= (q h, hy) Ya, A, Aa 1 2 3) 


Figure 9: Measured Orientation Vector[BACHO96B ]. 


The measurement of the local vertical and the earth's local magnetic field are represented 
as the 3-dimensional unit vectors given in (eq. 3.21) and (eq. 3.22), respectively. 


h = (hh (eq. 3.21) 


pi) 

b= (b,b,b;) (eq. 3.22) 

These vectors are then combined to form the complete measured orientation vector given 
by 

= (h,h,h,b,b,b,) (eq. 3.23) 


A 


The filter uses the unit orientation quaternion, 4, which is set when the system is 


initialized, to calculate the computed measurement vector, Y. The computed 


measurement vector is given by 
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. A | 
y(4) = (4 må,4 ng) (eq. 3.24) 


where m is the earth's unit gravity vector in earth coordinates and n is the earth's unit 
magnetic field vector in earth coordinates. Both the m and n vectors are rotated into body 
coordinates in order to permit comparison with the measured orientation vector given by 
(eq. 3.23). The error is then computed by taking the difference between the measured 


vector in (eq. 3.23) and the computed vector in (eq. 3.24), and is given by the expression 


5ጮ(2)6,.1 = 2(2)6,.1 - "0... (eq. 3.25) 

One version of the quaternion filter uses an approach known as the “method of steepest 
descent" to minimize the error in the computed orientation quaternion. The notion of a 
mathematical gradient is required to implement the method. The gradient is defined as the 
vector partial derivative of the squared error criterion function which is defined as the 


square of (eq. 3.25) and is given by 


2 ገ a 
®(9)1,1 = € (9) ,6E 6,1 (eq. 3.26) 
The next step of the filter multiplies (eq. 3.25) by the feedback gain matrix K,, which, 


for this implementation, is defined as 


T 
Aue, = K 4x4 2X y 6 (eq. 3.27) 
where, for the steepest descent 
K ene —kI 2 (eq. 3.28) 


and [MCGH67] 
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T |, 
X ፲፦ = (eq. 3.29) 
2 146 
Appendix A presents the derivation of the X matrix given in (eq. 3.29). The result of the 
top-half of the filter can then be given by 
HERE] (eq. 3.30) 


Substituting (eq. 3.27) into (eq. 3.30), gives 


e Ti 
de = Lara A Der rn) 


The gradient of the error criterion function given in (eq. 3.26) 1s defined as 


(eq. 3.32) 


T 
vean - (33 522 322. 3.) - 2x, ce Dei 


9260 991 992 9ና3 
Appendix B presents the derivation of the gradient given in (eq. 3.32). Now, substituting 


(eq. 3.32) into (eq. 3.31), yields 


de = Kara Da, (eq. 3.33) 


Thus, this new expression gives the output from the top-half of the filter. 


The vector given by (eq. 3.33) will be used to correct the computed value for g which 


is calculated using the bias-corrected output of the angular rate sensors in the lower left- 


hand portion of the filter, Figure 10. The expression for d is given by [COOK92] 
q- 39 ወ (ዩባ. 3.34) 


ae B 
where q is the orientation quaternion computed on the previous cycle of the filter and © 


is the body coordinate angular rate sensor output, (p q r), corrected for the estimated 
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bias, (p p 4p ry) . Using (eq. 3.33), the output of the top-half of the filter, to correct (eq. 
3.34), gives 

ያ = 4-K4,4V0(2)4,4 (eq. 3.35) 
The resulting vector from (eq. 3.35) is then numerically integrated and normalized, yielding 


the next approximation for the orientation quaternion, 7. The approximation, 4 , can then 


be used to correct the graphical representation of a tracked object within a VE simulation. 


Estimated Bias 
(5, 0, 7) 









(4 4 9 43) 


Angular Rate Sensor 


(par) (cos ,V in?) 


Figure 10: Angular Rate Sensor Output [BACH96B]. 
C.. SUMMARY 


The filter presented above offers significant improvements over filters using Euler 
angles. No singularities exist for any orientations, no trigonometric functions are required, 
and unit quaternions make calculating inverses a simple matter. This type of filter would 


be most desirable for applications which track human limb segments, eliminating the 
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vertical axis singularities which are encountered frequently when Euler angles are used in 
human motion simulations. 

The next chapter presents a Lisp implementation of the quaternion filter. An inertial 
system, consisting of accelerometers, angular rate sensors, and a 3-axis magnetometer, is 


simulated to test the convergence and stability of the filter. 


32 


IV. QUATERNION FILTER IN LISP 


In order to validate the theory presented in the previous chapter, a simulation model was 
implemented in Lisp. The model simulates output from a static inertial sensor and uses the 
quaternion filter to correct the resulting orientation quaternion. To simulate error, the filter 
is initialized to an estimated orientation quaternion which can be offset from the sensor's 


orientation by as much as 90°. This, of course, is a simulation to evaluate the theory and 


performance of the filter. One would never expect to see deviations as high as 90? in actual 
implementations. A deviation that high would suggest a poorly designed filter. Since the 
inertial sensor remains static throughout the simulation, the user sets the orientation 
quaternion which represents the orientation of the sensor in space. For the test runs 
performed in this thesis, an orientation quaternion representing alignment along the x-axis, 
(0 1 0 0), was chosen. This choice was arbitrary; any quaternion would work just as well. 
The known orientation quaternion of the sensor 1s then used to compute the output of the 
simulated inertial sensors and magnetometer. Using the outputs from the simulated inertial 
system and the initial orientation quaternion estimate, the filter calculates corrections to the 
error. If working properly, the filter should respond by gradually converging upon the 
orientation quaternion of the sensor, in this case (0 1 0 0). 

The speed at which the filter converges depends upon the convergence method used. 
Two different iterative convergence methods were tested in the filter. The “method of 


steepest descent’, utilizing the mathematical notion of a gradient, was used and applied as 
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described in the previous chapter. Also implemented was the Gauss-Newton method 


[MCGH67] given by 


1 -1 3» 
ለ2 er; sån Vo (eq. 4.1) 


where X represents the same matrix given in (eq. 3.29) 


dy: 
ys si 
71456 


> 
and Vo is the gradient given by (eq. 3.32) 


vor) - (29. 29. 89 at. - ox 


T " 
^ 3 ? ? e 
920 991 993 993 4x6 6x1 


Substituting (eq. 3.32) into (eq. 4.1) yields 


Aja. = -| T NEHME mm. (eq. 4.2) 
When using the Gauss-Newton method, (eq. 4.2) represents the output from the top-half of 
the filter and is used to correct the rate output from the angular rate sensor in the lower left- 
hand portion of Figure 8. It should be noted, however, that the form of the Gauss-Newton 
equation in (eq. 4.2), with a scalar multiplier of -1, is only applied to noiseless, perfect data. 
That is, (eq. 4.2) treats accelerometer and magnetometer data as if they were perfect 
measurements of m and n, the gravity and magnetic field vectors, respectively. Since in the 


real world this is hardly ever the case, when dealing with data corrupted by noise, a scalar 


multiplier & is used, defined as 


a = kAt (eq. 4.3) 
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where 0<a<1 _. The resulting numerical integration equation becomes 


2 m IB D er 
መ መ መ. @At+a[X X] X e(q,) (eq. 4.4) 


yielding the next approximation for the orientation quaternion. 
A. THE SIMULATION MODEL 

The quaternion filter takes five inputs, namely, accelerometer, angular rate, and 
magnetometer sensor output describing measured orientation data, a delta-t, and the last 


computed value of the orientation quaternion, q-hat. Accelerometer output is defined as a 


X, 7 ütqw-rv«* gsinO (eq. 4.5) 
p Y c ur-— pw —gsinocosO (eq. 4.6) 
Z, = W+pv-ug- gcosbcos® (eq. 4.7) 


To remain a purely quaternion implementation, the Euler angle representations for the 
gravity vector components in (eqs. 4.5, 4.6, 4.7) are replaced by equivalent quaternion 
expressions. This is done by rotating the earth-based gravity vector, m, into body 
coordinates using the orientation quaternion of the inertial sensor. The resulting quaternion 
representation for the gravity vector, in body coordinates, is analogous to the 

gsin8,-gsingdcos@ , -gcosdcos@ terms above. The Lisp implementation of the 
quaternion representation for (eqs. 4.5, 4.6, 4.7) is shown in Figure 11, taken from the take- 


accelerometer-reading method of the inertial-sensor class located in the file sensor.lsp in 


Appendix C. 
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(body-gravity-vector (rotate-vector | 
(quaternion-inverse (orientation-quaternion 
inertial-sensor)) *m*)) 


(ax (+ u-dot (* wg) (* -1 vr) (second body-gravity-vector))) 
(ay (+ v-dot (* u r) (* -1 w p) (third body-gravity-vector))) 
(az (+ w-dot (* v p) (* -1u q) (fourth body-gravity-vector)))) 


Figure 11: Code Fragment Showing Accelerometer Output Using 
Quaternions. 


Note that (eq. 3.19) 


Protated ” PPI 


is a rotation of a vector, p, from body coordinates to earth coordinates. Since the gravity 
vector is rotated from earth coordinates to body coordinates, the inverse of (eq. 3.19) is 


applied, namely, 


"body = 4 Moartn? (epe) 
This is done by using the function rotate-vector(). rotate-vector() takes a unit quaternion 
and an arbitrary vector and applies the quaternion rotation in (eq. 3.19). To effect the 
reverse rotation, as is required for the gravity vector, the inverse of the orientation 
quaternion is passed to rotate-vector() as shown in Figure 11. The resulting rotation is the 
desired earth-to-body vector rotation given in (eq. 4.8). 
Magnetometer output is generated by a magnetometer class [FREY96B, MCGH96C]. 
Analogous to the simulated accelerometer reading, simulation of the magnetometer 
requires that the earth's magnetic field vector, n, expressed in earth coordinates, be rotated 


to body coordinates. Before this can be done, however, the magnetometer simulation 
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incorporates the deviation and dip angle [FREY96B] characteristics of the earth's magnetic 
field in the local Monterey, California area. Magnetic deviation is defined as the difference 
between the north compass heading and the true geographic north at a given location on the 
earth’s surface. Monterey requires a correction of 15° [FREY96B, MCGH96C]. The lines 
of earth’s magnetic force are parallel to the surface at the earth’s equator. However, as the 
lines approach the magnetic poles, they become increasingly vertical. Dip angle, is the 
correction for the measure of the local downward deflection of the magnetic field. In 
Monterey, a correction of -60° is applied [FREY96B, MCGH96C]. The correction for the 
deviation and dip angle [FREY96B, MCGH96C] to the magnetic field vector is 
implemented in the code fragment shown in Figure 12. 


(defun earth-magnetic-field-unit-vector (deviation dip-angle) 
(rest (rotate-vector (equivalent-quaternion deviation (- dip-angle) 0) 
(0 10 0)))) 


‚normalized earth magnetic field vector in earth coordinates for Monterey, CA. 
(setf *n* (cons 0 
(earth-magnetic-field-unit-vector (deg-to-rad 15) (deg-to-rad 60)))) 


Figure 12: Deviation and Dip Angle Corrections for the Earth's 
Local Magnetic Field [FREY96B, MCGH96C]. 


Once corrected, the normalized magnetic field vector, n, is rotated to body coordinates 
using the rigid body's orientation quaternion, similar to the gravity vector, m, in the 
accelerometer simulation. Angular rate sensor output, for this implementation, was kept at 
(0 0 0), since the inertial sensor remains static throughout the simulation. Together, the 
values calculated for each sensor simulates the output from an inertial sensor at rest and 


constitutes the measured orientation vector in the quaternion filter. 
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B. SIMULATION RESULTS 


Each convergence method has its own test-filter function used to begin the simulation. 
The user provides values for k (gradient method), or multiplier (Gauss-Newton method), 
the desired initial offset of the estimated quaternion from the orientation of the sensor, 
maximum number of iterations, an error tolerance, and a delta-t for the Euler integration 
portion of the filter. The test-filter function for the gradient method is shown below in 


Figure 13. 


(defun test-filter-gradient (k-value offset iterations error-threshold delta-t) 

(setf *k* k-value) 

(setf Sensor (make-instance ‘inertial-sensor)) 

(initialize Sensor) p 

(calibrate-magnetometer Sensor -1 1 -1 1 -1 1) 

(let*  ((accelerometer (take-accelerometer-reading Sensor)) 
(magnetometer (take-magnetometer-reading Sensor)) 
(angular-rate (take-angular-rate-sensor-reading Sensor)) 
(q-hat Offset) 

(delta-t delta-t) 
(error 1)) 
(do ((x 1 (1+ x))) 
((> x iterations)) 
(format t "me Yo e. Op Fe ee ke Iteration ~A ቾቹቹችቶቹችችችችች..፡5"' X) 
(if (>= error error-threshold) 

(setf output (quaternion-filter-gradient accelerometer 
magnetometer 
angular-rate 
q-hat 
delta-t) 

q-hat (firstn 4 output) 

error (error-difference (lastn 6 output))) 

(setf x (1+ iterations)))))) 


Figure 13: Gradient Convergence Method. 


The function instantiates an instance of an inertial sensor, which has a super class of 
quaternion-rigid-body. Associated with the new inertial sensor object is a slot, inherited 


from quaternion-rigid-body, called posture. Posture is a vector containing both position 
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and orientation information, (xe ye ze q0 q1 q2 q3), for the object instance. The default 
posture is set to (0 0 0 0 1 O 0), corresponding to a position at the origin of the earth 
coordinate system and an orientation quaternion of (0 1 0 0), as shown in Figure 14 below. 


(defclass quaternion-rigid-body () 
( 


;the vector (xe ye ze q0 q1 q2 q3). 
(posture 
:initform ‘(0 0 00 10 0) 
:Initarg :posture 
:accessor posture) 


Figure 14: Posture Slot Default Value. 


The quaternion-rigid-body class also has a slot called orientation-quaternion. Orientation- 
quaternion contains the last four values of the slot posture, corresponding to the quaternion 
portion of the rigid body state. This slot is then used to rotate all vectors within the filter 
implementation. 

After the test function initializes the sensor and calibrates the magnetometer, readings 
from each of the three sensors of the inertial tracking system are taken. These values are 
assigned to variables corresponding to each of the three different sensors. This is done only 
once in this implementation of the simulation because the sensor remains static. Therefore, 
the posture of the sensor does not change, requiring no updates to the filter be made. A real 
system would need to take readings from the individual sensors on every iteration to correct 
for varying orientations of a moving sensor package. 

The test function then enters a loop which calls the appropriate version of quaternion- 
filter() until one of two conditions are met. The loop can either terminate after completing 
the maximum number of iterations, whether or not the filter has converged, or when the 


orientation-quaternion estimation is within the specified error-tolerance. The values of all 
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critical variables are printed on every iteration to allow for easy evaluation of filter 
performance. 

As mentioned above, two convergence methods were tested in the implementation of 
the quaternion filter. The gradient method, presented in Chapter 3, guarantees that a linear 
convergence to a local minimum will occur given a sufficiently small value for k. The 
value of k represents the size of the step taken along the gradient vector towards the 
minimum value, where the gradient vector lies perpendicular to the contour lines of a given 
surface. Large values for k mean large steps with faster convergence, but the possibility of 
overshooting the local minimum. Small values for k mean smaller steps with slower 
convergence, and a reduced chance of overshooting. An optimal value for k can be chosen 
using a method called the Newton-Raphson iteration [MCGH67]. This method was not 
used in the this simulation and is beyond the scope of this thesis. Filter performance, using 


the gradient method, was good. The filter was given a fairly large error. Specifically, an 


offset of 20? from the orientation quaternion of (0 1 0 0), given by (0 .939692621 


.342020143 0), was used to initialize the filter. The filter converged in an acceptable 
number of iterations with k values in the range 0.7 € k € 1.2 , averaging 22 iterations, 


with the best performance at k = 1.2, 17 iterations. Using an error of 10°, (0 


.9848077530 .173648177 0), yielded better results, as expected. The same range of k 


values averaged 15 iterations to converge with k = 1.2 being the best, converging in 12 


iterations. The complete test runs are included in Appendix D. 
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Test runs using the Gauss-Newton method well out-performed the gradient method. 
Gauss-Newton converges quadratically [MCGH67], often requiring only one iteration for 
small errors. Test runs, similar to the gradient method, were performed using the perfect 
sensor multiplier of -1. It should be noted that the call to test-filter-gauss-newton() is made 


with the multiplier set to 10 vice 1. Referring to (eq. 4.3) 

a = kAt 
the multiplier œ is expressed as the product of a constant, k, and At. Since delta-t is 
treated as a separate variable in the code, set to 0.1 in this simulation, the value given to the 


multiplier should cause (eq. 4.3) to equal 1 when performing the numerical integration in 


(eq. 4.4) 


i LE 85:21 2 2 
psa. Tus tips wAt+a[X X] X e(9,,) 
Therefore, a value of 10 for the variable multiplier yields the correct value for (eq. 4.3). 
Also, « is actually negative, but this is taken care of in the code, when calculating the value 


of (eq. 4.2) 


aa LAA e 
14x1 7 |X ፳14,4 4x69 (4361 


as shown in Figure 15 


(delta-q  (scalar-multiply-vector (* -1 multiplier) 
(post-multiply X-squared-inv (post-multiply X-trans error)))) 


Figure 15: Gauss-Newton Equation. 
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This was done in order to simplify the function call, avoiding inadvertent calls with positive 


values for the multiplier. Using the same 10° and 20° errors for the initial orientation 
quaternion estimation as the gradient method test runs, the Gauss-Newton method gave the 


following results, shown in Figure 16 and Figure 17. 


(test-filter-gauss-newton 10 10-degrees 10 .001 .1) 
initial q-hat (0 0.984807753 0.173648177 0) 


dolo Tteration 1 eee 


new-q-hat (5.62901044742602E-4 0.999968870927989 0.0045325612552949 - 
0.00643398833417277) 


jk ck ok ok ok ok ok ck ok Iteration 2 sk ok ok ok k kK k Kk k 


new-q-hat (-2.78220979546233E-5 0.999999999558864 -6.85807994618875E-6 - 
7.82114313870801E-6) 


Figure 16: Gauss-Newton Iteration with 10-degrees Error 


(test-filter-gauss-newton 10 20-degrees 10 .001 .1) 
initial q-hat (0 0.939692621 0.342020143 0) 


Kk kkk k k kk Iteration 1 K kk kk kk kk k 


new-q-hat (-0.00212904587800956 0.999131190052156 0.0337658829594332 - 
0.0243351058469248) 


OK 2 ck ok ok ok zk ok k Iteration 2 vk kk k k k k kkk 


new-q-hat (-7.57124440093844E-4 0.999999679561069 -1.35855048017928E-4 - 
2.21774091515007E-4) 


xoekeleleleleek Tteration 3 #444 kk kkk 


new-q-hat (-1.06750499371424E-8 0.999999999999999 -1.43682207139414E-8 
4.92518736799743E-8) 


Figure 17: Gauss-Newton Iteration with 20-degrees Error. 


C. SUMMARY 

The theory presented in Chapter 3 has been shown to be sound, as evidenced by the 
results obtained in the test runs presented. Two methods of convergence were successfully 
implemented. The gradient method showed acceptable results, converging quickly for 
large errors in the initial estimation of the orientation quaternion. Gauss-Newton, on the 


other hand, resulted in quadratic convergence, requiring only 9 iterations to converge for 
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an initial error of 89° ! Given the initial test results, it can be concluded that, the quaternion 
filter is a viable alternative to Euler angle based filters, especially in the realm of human 
body tracking. The errors used in the test runs were much larger than should ever be 
encountered in an actual inertial tracking system implementation. The quaternion filter, 
given a reasonably accurate inertial tracking system, should never allow the orientation 
quaternion to be off as much as was shown in this thesis. The next chapter presents the 
results of quantitative and qualitative analysis done on an existing inertial tracking system, 


the Angularis inertial tracker, manufactured by InterSense, Inc. [INTE97]. 
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V. RESULTS 


A. ANGULARIS INERTIAL TRACKING SYSTEM 


The Angularis tracker is a palm-sized plastic block which contains three orthogonal 
angular rate sensors, three orthogonal accelerometers, and a two-axis magnetometer to 
determine the angular orientation of the human head [FREY96A]. Built initially for HMD 
applications, the extensibility of the Angularis to entire body tracking is quickly becoming 
a possibility. Inertial systems, like the Angularis, do not suffer from the traditional 
shortcomings of sensors based on mechanical, electromagnetic and acoustic technologies. 
They are self-contained, requiring no outside source to calculate spatial orientation, making 
them immune to outside interference and extending their range beyond that of any 
traditional tracking system. Until recently, the only limitation to applying inertial 
navigation technology to body tracking was the fact that inertial sensors were big, bulky 
unwearable devices. However, with the advent of state-of-the-art, micro-machined inertial 
sensor components, this concern is no longer a factor. 

The question being investigated is whether or not the Angularis, in its present 
configuration, would be applicable to human body limb segment tracking within a VE 
simulation. Since the sensor was originally built to be worn as a head tracking device, 
inherent assumptions in the design of the sensor and filtering technique could possibly 
prove to be a limiting factor in extending its use to entire body tracking. 

The system consists of the inertial tracker and a computer processing unit which 
receives data from the sensor, processes it, and delivers filtered orientation data to the host 


computer via an RS-232 cable connection. In its present configuration, data from the 
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individual sensors, within the plastic block, is inaccessible. A “hard-coded” Kalman 
filtering technique is used to filter data from the sensor [INTE96]. The filter is “hard- 
coded" in the sense that it is impossible to apply any outside filter, such as the quaternion 
filter, to the sensor output. The only choice given to the user is between full-order or 
reduced-order Kalman filtering [INTE96]. Reduced-order is the default, allowing the 
system to run above 500 Hz and delivering almost the same performance as full-order 
Kalman filtering [INTE96]. Once the filtered data is passed to the host computer, VE 
software running on the host can use the orientation data to update graphical 
representations of tracked objects. 

In order to test the performance of the Angularis sensor, mechanical tilt-table tests 


were performed at varying rates of rotation. The sensor was strapped onto the tilt-table and 


allowed to measure the roll angle as the unit was rotated through 45°. The roll was 
performed two times at each rate, allowing the sensor to stabilize at the end of each roll for 
20 seconds. The results are shown in Figure 18, Figure 19, and Figure 20. The Angularis 
performed well, evidenced by the outputs shown on the graphs. However, the sensor does 
not correct for errors at the end of the roll as in the SANS filter tests by [BACH96A, 
ROBE97]. Specifically, while the SANS filter graphs show a gradual correction being 
applied to the angle as the system stabilizes at the end of the roll, no such corrections are 
made by the Angularis. Whatever angle is returned at the end of the roll, no matter what 
the error, the sensor "locks on" to that reading and does not correct. This is shown by the 


flat line at the end of each roll in the graphs. This lack of immediate correction for the error 
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may be attributed to the fact that the sensor was originally designed for HMD applications. 
In his paper, [FOXL94] explains the drift correction method applied to the first prototype 
of the Angularis sensor. Using the heuristic that the human head pauses every 10 seconds 
in a typical HMD simulation, [FOXL94] explains that this pause would allow the fluid- 
filled inclinometer to settle to its correct pitch and roll in approximately 1/4 second. When 
this occurred, the orientation values would be corrected and reset. However, the error 
would not be corrected all at once. Gradual correction of the error was done to prevent 
jarring the user, a real concern with HMD applications [FOXL94]. This design feature may 
explain the output shown in the graphs above. 

The fact that individual sensor outputs, from the components of the inertial system, are 
not accessible makes the overall system limited in its application. It is doubtful that body 
tracking applications would fare well with the current configuration. A qualitative test was 
performed using the software provided by the manufacturer. The software represented the 
sensor block as a virtual cube which moved in response to the movements applied to the 
actual sensor. It was observed that the graphical representation would not correspond to 
movements applied to the sensor after just a few arbitrary rotations. When the sensor was 
placed flat on the table, the graphical representation manifested the errors which had 
accumulated. This error could be corrected by jiggling the sensor until the graphical 
representation matched the orientation of the actual sensor. This anomaly may be able to 
be corrected if the system had allowed the application of an alternative filter. Graphs 


representing the output from random rotations are shown in Figure 21 and Figure 22. The 
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sensor was rotated arbitrarily and placed flat on a table and then rotated again. This was 
repeated several times to simulate the random motions that may be encountered when 
tracking a human within a VE. The figures clearly show that when the sensor was placed 
in its reference position, flat on the table, it would report erroneous orientation data. This 
is shown by the flat line portions of the graphs which represent no rotations being applied 
to the sensor. Clearly, these lines are not at their reference position as they should be when 
the sensor is at rest. These results are concurrent with the performance observed when 
using the manufacturers graphical software. The error corrections being applied to the 
sensor output could not keep up with the rapid random rotations being applied. Since 
tracking human limb segments will doubtlessly encounter such random motions, for such 
applications the sensor needs to be re-configured to allow the application of an alternative 


filtering technique such as the quaternion filter presented in this thesis. 


B. QUATERNION FILTER 

The results of the tests performed on the implementation of the quaternion filter were 
better than expected. The filter performed extremely well under extreme error conditions. 
Deviations up to 90? were corrected by the filter. Applying the Gauss-Newton iteration 
method provided quadratic convergence to the correct orientation quaternion, in some 
cases in only one iteration. Overall, the thesis was a success, in that the theory for the 
quaternion filter was proven correct and an implementation of the filter now exists which 


can be applied to any VE simulation incorporating inertial tracking sensors technology. 


SS 


The next, and final chapter, presents some final thoughts and recommendations for future 


work relating to inertial tracking of limb segment angles. 
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VI. SUMMARY AND CONCLUSIONS 


A. SUMMARY 


In this thesis, a quaternion filter is presented as an alternative to filters using Euler 
angles. This is because Euler angle implementations could encounter singularities that 
make them undesirable for human body tracking applications. Specifically, filters 


designed to use Euler angles experience divide-by-zero errors when calculating estimates 


of orientation at elevation angles of +90° (as shown in Figure 7). This limitation is critical 
in simulations una: the tracking of human limb segments which often rotate through 
vertical ns The: dx: of this thesis shows that the quaternion filter proposed by 
[MCGH96A] provides a sound and viable solution to the Euler angle singularity problem, 
while at the same time simplifying the required filter computations. 


Two different quaternion filter convergence methods were implemented and tested. 


The gradient descent method and the Gauss-Newton method [MCGH67] were tested under 


extreme error conditions of up to 90°. In both cases, the filter was able to quickly and 
accurately correct to the appropriate orientation quaternion. The gradient descent method 
provided gradual convergence, usually needing dozens of steps, while the Gauss-Newton 
method converged quadratically, often requiring just a single iteration for small initial 
errors. The results of the initial tests provide a decisive demonstration of the quaternion 
filter’s applicability to VE simulations utilizing inertial sensors for human body tracking. 
The Angularis inertial tracker [INTE97] was investigated as a possible alternative to 


traditional tracking methods by mechanical, electromagnetic, and acoustic tracking 
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technologies. The Angularis was built specifically as an inertial tracker for HMD 
applications. It was one of the goals of this thesis to investigate the viability of extending 
the use of the Angularis inertial sensor to human limb segment tracking. Tilt-table tests 
resulted in very accurate readings when this tracker was given steady, consistent rotations 
(shown in Figures 18, 19, and 20). However, when tested by hand, using rapid random 
rotations and orientations, the system had difficulty returning to its reference position 
(shown in Figures 21 and 22). These results suggest that the Angularis inertial system, in 
its current configuration, would not perform as well as would be required for human limb 
segment tracking applications. However, it is the author's understanding that this 


limitation can be removed by the manufacturer at the request of users. 


B. CONCLUSIONS 


Ithas be shown that filters based on quaternions have significant advantages for human 
body tracking in comparison to the more common Euler angle approach to attitude 
estimation. Two different convergence methods were investigated and tested in 
implementing the quaternion filter. Although both yielded acceptable results, the Gauss- 
Newton method was shown to be superior. Resulting in quadratic convergence, Gauss- 
Newton appears to be the preferred convergence method for further investigations 
concerning quaternion filters. 

The integration of the quaternion filter with the Angularis inertial system proved to be 
an impossibility. The system, as it currently exists, does not allow for the application of 
independent filtering software. Better filtering techniques are required if the sensor is to be 


applied to human limb segment tracking. It has been demonstrated that a filter using 
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quaternions would be a viable solution to the filtering deficiencies experienced in the 


Angularis tests. A coupling of both entities could result in a very competitive system. 


C. FUTURE WORK 


The quaternion filter still requires extensive testing under realistic conditions using 
actual inertial sensors implemented in a VE simulation. This thesis presented a simulation 
incorporating "perfect sensors". No attempt was made to reproduce noise levels similar to 
those present in real sensor output. Research of the effects of noise on the filter's 
performance is still needed and required. Also, the use of dynamic objects must be 
investigated. The current evaluation was conducted using a simulated static sensor with a 
constant orientation. This type of simulation does not realistically demonstrate the 
environment in which the filter will be required to operate. In order to be useful, the filter 
must be capable of performing under the dynamic conditions of a typical VE simulation. 
The Angularis sensor could also be further improved by reducing its current size. The 
Angularis is certainly an improvement over traditional inertial systems, but is still a bit too 
large for reasonable incorporation into a body suit capable of tracking an entire human 
body. It is the author’s opinion that, notwithstanding the current limitations of the 
Angularis sensor, the cutting edge tracking systems of the future will incorporate similar 
inertial sensors with filtering being done by a quaternion filter similar to, if not the same as, 
the one presented in this thesis. It is hoped that the work of this thesis makes a significant 


contribution toward the realization of such systems. 
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APPENDIX A. DERIVATION OF X MATRIX 


The X matrix is defined by (eq. 3.29) as 


Qv. 
99) 4x6 


The elements of the 4x6 matrix come from the partial derivatives of the components of the 


computed measurement vector, 3(9) , given by (eq. 3.24) 
Pra ፈ፦1 ል Al A 
y(q) = (4 mq,q nq) 


So, taking the partial derivative of (eq. 3.24) with respect to qo, the result is 


<< c (q ma, q nq) (eq. A.l) 
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09 .. (1000) (eq. A.3) 
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ጋ - 1 
94 |. 2 (1000) (eq. A.4) 
do 
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Likewise, for q1, q2, and q3. 
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where the corresponding quaternion partial derivatives are 


9q = (0100) 
dq] 
94 - (0010) 
993 
09 - (0001) 
993 


and, the partial derivatives of the inverse are given by 


— = (0-100) 


መጨ = (00-10) 
2 
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(eq. A.5) 


(eq. A.6) 


(eq. A.7) 


(eq. A.8) 


(eq. A.9) 


(eq. A.10) 


(eq. A.11) 


(eq. A.12) 


-1 
92 _ (000-1) (eq. A.13) 
43 


The partial derivatives as defined in (eq. A.2), (eq. A.5), (eq. A.6), and (eq. A.7) result in 


the partial derivatives of the m and n vectors with respect to qo, q1, q2, and q3, respectively. 


Taking the results computed above, the X¢,,4 matrix can be constructed as follows 


> > > > 
a 2 (eq. A.14) 
99, 99, 99, 993 ¿A 


Note that the partial derivatives are column vectors in the X matrix, and that the transpose 


of X is required when applied in the filter. Thus, (eq. 3.29) becomes 
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APPENDIX B. DERIVATION OF GRADIENT 


In the filter the gradient of the error criterion function 0(@) , is given by (eq. 3.32) 


db 96 96 90 Y _ LT ..« 
Vo(q ) = = E groaa dqa -1 = 244 AD 6x1 





where $(4)is given by (eq. 3.26) 
^ መ T ^ ^ 
0(4)1,41 = © (Dy x6E( Dex] 
In order to simplify the derivation of the gradient, consider the case of 1 dimensional 


orientation vectors, where the measured vector is Yq = (hi) and the calculated vector 


83. በጸ (hy ). The error then becomes 


e(9) = hı-h, (eq. B.1) 


and 


^ 2 ^ 2 R2 ^ 2 
ዕ(2) = € = (hy-h,) = hy-2hyhy +h; (eq. B.2) 


Taking the partial derivative of (eq. B.2) with respect to qo yields 


^ ^ ^ 


» d dh; dh dh; 
= = i = 2 ish ) = 222 (4) (eq. B.3) 


^ h ^ 
Qo 1940 - 252 
Likewise, for the partial derivatives with respect to q1, q^, and q4. Thus, extending this 


result to the 6 dimensional case and arranging the partial derivatives in matrix form yields 
(2). 
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APPENDIX C. SIMULATION MODEL CODE 


File: test-filter.1sp 


; positive offsets from the positive x-axis 

(setf O-degrees '(0 1 0 0)) 

(setf 1-degree "(0 0.9998477 0.017452406 0)) 

(setf 2-degrees '(0 0.99939083 0.034899497 0)) 

(setf 3-degrees '(0 0.99862953 0.052335956 0)) 

(setf 4-degrees '(0 0.99756405 0.069756474 0)) 

(setf 5-degrees '(0 .9961946981 .0871557427 0)) 

(setf 10-degrees '(0 .9848077530 .173648177 0)) 

(setf 20-degrees '(0 .939692621 .342020143 0)) 

(setf 30-degrees '(0 .8660254037 .5 0)) 

(setf 40-degrees '(0 766044443119 .642787609687 0)) 
(setf 50-degrees '(0 .642787609687 .766044443119 0)) 
(setf 60-degrees '(0 .5 .8660254037 0)) 

(setf 70-degrees (0 .342020143 .939692621 0) 
(setf 80-degrees '(0 .173648177 .9848077530 0)) 

(setf 85-degrees '(0 .0871557427 .9961946981 0)) 
(setf 89-degrees '(0 .017452406 .999847695 0)) 

(setf 90-degrees '(0 O 1 0)) 


(defun error-difference (error) 
(apply #'+ (mapcar #'square error))) 


(defun test-filter-gradient (K-value offset iterations error-threshold delta-t) 

(setf *k* k-value) 

(setf Sensor (make-instance 'inertial-sensor)) 

(initialize Sensor) 

(calibrate-magnetometer Sensor -1 1 -11 -1 1) 

(let* ((accelerometer (take-accelerometer-reading Sensor)) 
(magnetometer (take-magnetometer-reading Sensor)) 
(angular-rate (take-angular-rate-sensor-reading Sensor)) 
(q-hat offset) 

(delta-t delta-t) 
(error 1)) 
(format t "~%initial q-hat ~A ~%" q-hat) 
(do ((x 1 (1+ x))) 
(( x iterations)) 
(format t "ur Oo me Yo FFF KKK KKH Iteration ZA RK E O" X) 
(if (>= error error-threshold) 

(setf output (quaternion-filter-gradient accelerometer 
magnetometer 
angular-rate 
q-hat 
delta-t) 

q-hat (firstn 4 output) 
- error (error-difference (lastn 6 output))) 
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(setf x (1+ iterations)))))) 


(defun test-filter-gauss-newton (multiplier offset iterations error-threshold delta-t) 

(setf Sensor (make-instance 'inertial-sensor)) 

(initialize Sensor) 

(calibrate-magnetometer Sensor -1 1 -1 1 -1 1) 

(let* ((accelerometer (take-accelerometer-reading Sensor)) 
(magnetometer (take-magnetometer-reading Sensor)) 
(angular-rate (take-angular-rate-sensor-reading Sensor)) 
(q-hat offset) 

(delta-t delta-t) 
(error 1)) 
(format t "~%initial q-hat ~A -%" q-hat) 
(do ((x 1 (1+ x))) 
((> x iterations)) 
(format t Qo me YORE KK KK Iteration ~A ቶቾችችችችችችችችች.% "' X) 
(if (>= error error-threshold) 

(setf output (quaternion-filter-gauss-newton multiplier 
accelerometer 
magnetometer 
angular-rate 
q-hat 
delta-t) 

q-hat (firstn 4 output) 
error (error-difference (lastn 6 output))) 
(setf x (1+ iterations)))))) 
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File: sensor.lsp 


(defclass inertial-sensor (quaternion-rigid-body) 
((accelerometer 
:initform '(0 0 0) 
:accessor accelerometer) 
(angular-rate-sensor 
:Initform '(0 0 0) 
:accessor angular-rate-sensor) 
(magnetometer 
:initform (make-instance '3-axis-magnetometer) 
‘accessor magnetometer))) 


(defmethod take-accelerometer-reading ((inertial-sensor inertial-sensor)) 
(let* ((u (first (velocity inertial-sensor))) 
(v (second (velocity inertial-sensor))) 
(w (third (velocity inertial-sensor))) 
(p (fourth (velocity inertial-sensor))) 
(q (fifth (velocity inertial-sensor))) 
(r (sixth (velocity inertial-sensor))) 
(u-dot (first (velocity-growth-rate inertial-sensor))) 
(v-dot (second (velocity-growth-rate inertial-sensor))) 
(w-dot (third (velocity-growth-rate inertial-sensor))) 
(ql (fifth (posture inertial-sensor))) 
(q2 (sixth (posture inertial-sensor))) 
(q3 (seventh (posture inertial-sensor))) 
(body-gravity-vector (rotate-vector 
(quaternion-inverse (orientation-quaternion 
inertial-sensor)) *m*)) 


(ax (4- u-dot (* w q) (* -1 v r) (second body-gravity-vector))) 
(ay (4- v-dot (* u r) (* -1 w p) (third body-gravity-vector))) 
(az (+ w-dot (* v p) (* -1 u q) (fourth body-gravity-vector)))) 


(setf (accelerometer inertial-sensor) 
(normalize-vector (list ax ay az))) 
(accelerometer inertial-sensor))) 


(defmethod take-angular-rate-sensor-reading ((inertial-sensor inertial-sensor)) 
(setf (angular-rate-sensor inertial-sensor) 
(cons (fourth (velocity inertial-sensor)) 
(cons (fifth (velocity inertial-sensor)) 
(list (sixth (velocity inertial-sensor)))))) 
(angular-rate-sensor inertial-sensor)) 


; Max and min raw output over all orientations. 
(defmethod calibrate-magnetometer 
((inertial-sensor inertial-sensor) xmin xmax ymin ymax zmin zmax) 
(setf (x-bias (magnetometer inertial-sensor)) (/ (+ xmax xmin) 2) 
(y-bias (magnetometer inertial-sensor)) (/ (+ ymax ymin) 2) 
(z-bias (magnetometer inertial-sensor)) (/ (+ zmax zmin) 2) 
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(x-scale-factor (magnetometer inertial-sensor)) (/ (- xmax xmin) 2) 
(y-scale-factor (magnetometer inertial-sensor)) (/ (- ymax ymin) 2) 
(z-scale-factor (magnetometer inertial-sensor)) (/ (- zmax zmin) 2))) 


(defmethod normalize-magnetometer-measurement-vector 
((inertial-sensor inertial-sensor)) 
(let* ((vx (x-reading (magnetometer inertial-sensor))) 
(vy (y-reading (magnetometer inertial-sensor))) 
(vz (z-reading (magnetometer inertial-sensor))) 
(xbias (x-bias (magnetometer inertial-sensor))) 
(ybias (y-bias (magnetometer inertial-sensor))) 
(zbias (z-bias (magnetometer inertial-sensor))) 
(xscale (x-scale-factor (magnetometer inertial-sensor))) 
(yscale (y-scale-factor (magnetometer inertial-sensor))) 
(zscale (z-scale-factor (magnetometer inertial-sensor))) 
(wx (normalize-reading vx xbias xscale)) 
(wy (normalize-reading vy ybias yscale)) 
(wz (normalize-reading vz zbias zscale))) 
(normalize-vector (list wx wy w2)))) 


(defmethod take-magnetometer-reading ((inertial-sensor inertial-sensor)) 
(let* ((q-inv (quaternion-inverse (orientation-quaternion inertial-sensor))) 
(reading (rotate-vector q-inv *n*))) 

(setf (x-reading (magnetometer inertial-sensor)) (second reading) 
(y-reading (magnetometer inertial-sensor)) (third reading) 
(z-reading (magnetometer inertial-sensor)) (fourth reading) 
(magnetic-normal-vector (magnetometer inertial-sensor)) 

(normalize-magnetometer-measurement-vector inertial-sensor)) 

(magnetic-normal-vector (magnetometer inertial-sensor)))) 


(defclass 3-axis-magnetometer (quaternion-rigid-body) 
((x-reading :accessor x-reading) 
(y-reading :accessor y-reading) 
(z-reading :accessor z-reading) 
(x-bias :accessor x-bias) 
(y-bias :accessor y-bias) 
(z-bias :accessor z-bias) 
(x-scale-factor :accessor x-scale-factor) 
(y-scale-factor :accessor y-scale-factor) 
(z-scale-factor :accessor z-scale-factor) 
(magnetic-normal-vector :accessor magnetic-normal-vector))) 


(defun normalize-reading (value bias scale-factor) 
(/ (- value bias) scale-factor)) 
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File: quaternion-filter 


(defun earth-magnetic-field-unit-vector (deviation dip-angle) 
(rest (rotate-vector (equivalent-quaternion deviation (- dip-angle) 0) 
(0 10 0))) 


;normalized earth magnetic field vector in earth coordinates for Monterey, CA. 
(setf *n* (cons 0 (earth-magnetic-field-unit-vector (deg-to-rad 15) (deg-to-rad 60)))) 


;normalized gravity vector in earth coordinates. 
(setf *m* '(0 0 0 1)) 


‚estimated sensor bias in body coordinates. 
(setf *angular-rate-sensor-bias* '(0 0 0)) 


(defun calculated-measurement-vector (quaternion) 
(append (rest (rotate-vector (quaternion-inverse quaternion) *m*)) 
(rest (rotate- vector (quaternion-inverse quaternion) *n*)))) 


(defun make-X-transpose-matrix (quaternion vector) 
(list (append (rest (partial-derivative-qO quaternion (firstn 4 vector))) 

(rest (partial-derivative-q0 quaternion (cddddr vector)))) 

(append (rest (partial-derivative-q1 quaternion (firstn 4 vector))) 
(rest (partial-derivative-q1 quaternion (cddddr vector)))) 

(append (rest (partial-derivative-q2 quaternion (firstn 4 vector))) 
(rest (partial-derivative-q2 quaternion (cddddr vector) ))) 

(append (rest (partial-derivative-q3 quaternion (firstn 4 vector))) 
(rest (partial-derivative-q3 quaternion (cddddr vector)))))) 


; alternative way to compute partial derivatives 
(defun make-X-transpose-matrix-2 (quaternion m n) 
(list (partial-derivative-qO-2 quaternion (append m n)) 
(partial-derivative-q1-2 quaternion (append m n)) 
(partial-derivative-q2-2 quaternion (append m n)) 
(partial-derivative-q3-2 quaternion (append m n)))) 


(defun quaternion-filter-gradient (accelerometer 

magnetometer 

angular-rate 

q-hat delta-t) 

(let* ((measured-y (append accelerometer magnetometer)) 
(calculated-y (calculated-measurement-vector q-hat)) 
(error (vector-subtract calculated-y measured-y)) 
(X-trans (make-X-transpose-matrix q-hat (append *m* *n*))) 
(gradient-phi (scalar-multiply-vector 2 (post-multiply X-trans error))) 
(K (scalar-multiply-matrix (* -1 *k*) (unit-matrix 4))) 
(omega (cons '0 (vector-subtract angular-rate *angular-rate-sensor-bias*))) 
(q-dot (scalar-multiply-vector 0.5 (quaternion-product q-hat omega))) 
(q-hat-dot (vector-add q-dot (post-multiply K gradient-phi))) 
(new-q-hat (normalize-vector (vector-add q-hat 
(scalar-multiply-vector delta-t q-hat-dot))))) 
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(format t "~%q-hat ~A ~%" q-hat) 

(format t "~%measured-y ~A ~%" measured-y) 
(format t "~%calculated-y ~A ~%" calculated-y) 
(format t "~%error ~A ~%" error) 

(format t "-%gradient-phi -A -%" gradient-phi) 
(format t "~%new-q-hat ~A ~%" new-q-hat) 
(append new-q-hat error))) 


(defun quaternion-filter-gauss-newton (multiplier 
accelerometer 
magnetometer 
angular-rate 
q-hat delta-t) 
(let* ((measured-y (append accelerometer magnetometer)) 
(calculated-y (calculated-measurement-vector q-hat)) 
(error (vector-subtract calculated-y measured-y)) 
(X-trans — (make-X-transpose-matrix q-hat (append *m* *n*))) 
(X-squared-inv (matrix-inverse (matrix-multiply X-trans (transpose X-trans)))) 
(delta-q (scalar-multiply-vector (* -1 multiplier) 
(post-multiply X-squared-inv (post-multiply X-trans error)))) 
(omega (cons '0 (vector-subtract angular-rate *angular-rate-sensor-bias *))) 
(q-dot (scalar-multiply-vector 0.5 (quaternion-product q-hat omega))) 
(q-hat-dot (vector-add q-dot delta-q)) 
(new-q-hat (normalize-vector (vector-add q-hat 
(scalar-multiply-vector delta-t q-hat-dot))))) 
(format t "~%q-hat ~A ~%" q-hat) 
(format t "~%measured-y ~A ~%" measured-y) 
(format t "~%calculated-y ~A ~%" calculated-y) 
(format t "~%error ~A ~%" error) 
(format t "~%delta-q ~A ~%" delta-q) 
(format t "~%new-q-hat ~A ~%" new-q-hat) 
(append new-q-hat error))) 
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File: quat-rigid-body.lsp 


(defconstant *gravity* 32.2185) 
(defclass quaternion-rigid-body () 


( 

‚the vector (xe ye ze q0 ql q2 q3). 
(posture 

:initform (0000100) 

:Initarg :posture 

:accessor posture) 


the vector (xe-dot ye-dot ze-dot q0-dot q1-dot q2-dot q3-dot). 
(posture-rate 

‘initarg :posture-rate 

:accessor posture-rate) 


;the vector (u v w pq r) in body coordinates. 
(velocity 

‘initform '(0 0 0 0 0 0) 

Anitarg :velocity 

:accessor velocity) 


;the vector (u-dot v-dot w-dot p-dot q-dot r-dot). 
(velocity-growth-rate 
:accessor velocity-growth-rate) 


:the vector (Fx Fy Fz L M N) in body coordinates. 
(forces-and-torques 

‘initform (list 0 0 (- *gravity*) 0 0 0) 

:accessor forces-and-torques) 


:the vector (Ix Iy Iz) in principal axis coordinates. 
(moments-of-inertia 

:Initform "(1 1 1) 

initarg :moments-of-inertia 

‘accessor moments-of-inertia) 


(mass 

:Initform 1 

‘initarg :mass 

:accessor mass) 
(orientation-quaternion 

:Initform (0100) 

:accessor orientation-quaternion) 
(position-quaternion 

initform "(0 0 0 0) 

"accessor position-quaternion) 


(time-stamp 
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accessor time-stamp) 


;(0 x y z) in body coordinates for each node. 

(node-list 
:initform '((0 00 0) (0550) (0-5 5 0) (0 -5 -5 0) (0 5 -5 0)) 
:Initarg :node-list 
:accessor node-list) 


(polygon-list 
initform '((1 2 3 4)) 
:Initarg :polygon-list 
:accessor polygon-list) 


(x y z 1) in earth coordinates. 
(transformed-node-list 
:accessor transformed-node-list))) 


(defmethod initialize ((body quaternion-rigid-body)) 
(setf (transformed-node-list body) 
(mapcar #' (lambda.(node-location) (append (rest node-location) '(1))) 
(node-list body))) 
(setf (velocity-growth-rate body) (update-velocity-growth-rate body)) 
(setf (posture-rate body) (earth-velocity body)) 
(setf (time-stamp body) (get-constant-delta-t body))) 


(defmethod quaternion-move ((body quaternion-rigid-body) x y z q0 q1 q2 q3) 
(setf (posture body) (list x y z q0 q1 q2 q3)) 
(setf (orientation-quaternion body) (list q0 q1 q2 q3)) 
(setf (position-quaternion body) (append '(0) (list x y z))) 
(transform-node-list body)) 


(defmethod get-constant-delta-t ((body quaternion-rigid-body)) 0.1) 


(defmethod get-delta-t ((body quaternion-rigid-body)) 
(let* ((new-time (get-internal-real-time)) 
(delta-t (/ (- new-time (time-stamp body)) 1000))) 
(setf (time-stamp body) new-time) 
delta-t)) 


(defmethod update-rigid-body ((body quaternion-rigid-body)) 
(let* ((delta-t (get-constant-delta-t body))) 
(update-posture body delta-t) 
(setf (orientation-quaternion body) 
(list (fourth (posture body)) (fifth (posture body)) 
(sixth (posture body)) (seventh (posture body)))) 
(setf (position-quaternion body) 
(list O (first (posture body)) 
(second (posture body)) (third (posture body)))) 
(transform-node-list body) 
(update-velocity body delta-t) 
(update-velocity-growth-rate body))) 
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(defmethod update-velocity-growth-rate ((body quaternion-rigid-body)) 
(setf (velocity-growth-rate body) 
(multiple-value-bind 
(Fx Fy FZL MN uvwogr Iy Iz) 
(values-list 
(append 
(forces-and-torques body) 
(velocity body) 
(moments-of-inertia body))) 
(list (+ (* v r) (* -1 w q) (/ Fx (mass body)) 
(* -] (second (rotate-vector 
(quaternion-inverse (orientation-quaternion body)) 
(append "(0 0 0) (list *gravity*)))))) 
(+ (* w p) (* -1 u r) (/ Fy (mass body)) 
(third (rotate-vector 
(quaternion-inverse (orientation-quaternion body)) 
(append '(0 0 0) (list *gravity*))))) 
(+ (* u q) (* -1 v p) (/ Fz (mass body)) 
(fourth (rotate-vector 
(quaternion-inverse (orientation-quaternion body)) . 
(append '(0 0 0) (list *gravity*))))) 
(/ (+ (* (- Ty Iz) qr) L) Ix) 
(/ (+ (* C Iz Ix) r p) M) Iy) 
( (- CF C Ix Iy) p 9) N) I2))) 


(defmethod update-velocity ((body quaternion-rigid-body) delta-t) 
(setf (velocity body) 
(vector-add (velocity body) 
(scalar-multiply-vector delta-t (velocity-growth-rate body))))) 


(defmethod update-posture ((body quaternion-rigid-body) delta-t) 
(setf (posture-rate body) (earth-velocity body)) 
(setf (posture body) 
(append (vector-add (firstn 3 (posture body)) 
(scalar-multiply-vector delta-t (firstn 3 (posture-rate body)))) 
(normalize-vector (vector-add (cdddr (posture body)) 
(scalar-multiply-vector delta-t (cdddr (posture-rate body)))))))) 


(defmethod transform-node-list ((body quaternion-rigid-body)) 
(setf (transformed-node-list body) 
(mapcar f (lambda (node-location) 
(append (rest (vector-add 

(position-quaternion body) 

(rotate-vector (orientation-quaternion body) 
node-location))) 

(1)) 
(node-list body)))) 


(defmethod earth-velocity ((body quaternion-rigid-body)) 


(let* ((linear-velocity (append '(0) (firstn 3 (velocity body)))) 
(rotational-velocity (append '(0) (cdddr (velocity body)))) 
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(linear-earth-velocity 

(rotate-vector (orientation-quaternion body) linear-velocity)) 

(rotational-earth-velocity (scalar-multiply-vector 0.5 

(quaternion-product (orientation-quaternion body) rotational-velocity)))) 
(append linear-earth-velocity rotational-earth-velocity))) 


(defmethod print-body-posture ((body quaternion-rigid-body)) 

(format t E D 252 E ØRE 2 edo E U © Lo" 
(first (posture body)) (second (posture body)) (third (posture body)) 
(fourth (posture body)) (fifth (posture body)) (sixth (posture body)) 
(seventh (posture body)))) 


(defmethod print-body-orientation-quaternion ((body quaternion-rigid-body)) 
(format t "~,2,,,F ~,2,,,F ~,2,,,F ~,2,,,F ~%" 
(first (orientation-quaternion body)) (second (orientation-quaternion body)) 
(third (orientation-quaternion body)) (fourth (orientation-quaternion body)))) 
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File: partial-derivative.isp 


(defun partial-derivative-q0 (quaternion vector) 
(let ((q quaternion) 
(q-inv (quaternion-inverse quaternion)) 
(partial-q0 ‘(1 0 0 0)) 
(partial-q0-inv "(1 0 0 0)) 
(v vector)) 
(vector-add (quaternion-product partial-q0-inv (quaternion-product v q)) 
(quaternion-product q-inv (quaternion-product v partial-q0))))) 


; alternative method 
(defun partial-derivative-q0-2 (quaternion vector) 
(let ((qO (first quaternion)) 
(q1 (second quaternion)) 
(q2 (third quaternion)) 
(q3 (fourth quaternion)) 
(m1 (first vector)) 
(m2 (second vector)) 
(m3 (third vector)) 
(n1 (fourth vector)) 
(n2 (fifth vector)) 
(n3 (sixth vector))) 
(list (+ (* 2 q0 m1) (* -2 q2 m3) (* 2 q3 m2)) 
(+ (* 2 q0 m2) (* 291 m3) (* -2q03 ml1)) 
(+ (* 2 q0 m3) (* -2 q1 m2) (* 2 q2 m1)) 
(+ (* 2 q0 n1) (* -2 q2 n3) (* 2 q3 n2)) 
(+ (* 2 q0 n2) (* 2 g1 n3) (* -2 q3 n1)) 
( (* 2q0 n3) (* -291 n2) (* 292 n1))))) 


(defun partial-derivative-q1 (quaternion vector) 
(let ((q quaternion) 
(q-inv (quaternion-inverse quaternion)) 
(partial-q1 '(0 1 0 0)) 
(partial-q1-inv ‘(0 -1 0 0)) 
(v vector)) 
(vector-add (quaternion-product partial-q1-inv (quaternion-product v q)) 
(quaternion-product q-inv (quaternion-product v partial-q1))))) 


; alternative method 
(defun partial-derivative-31-2 (quaternion vector) 
(let ((qO (first quaternion)) 

(q1 (second quaternion)) 
(q2 (third quaternion)) 
(q3 (fourth quaternion)) 
(m1 (first vector)) 
(m2 (second vector)) 
(m3 (third vector)) 
(nl (fourth vector)) 
(n2 (fifth. vector)) 
(n3 (sixth vector))) 
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(list (+ (+ 2 q1 m1) (* 2 q2 m2) (* 2 q3 m3)) 
(+ (* 2 q0 m3) (* -2 q1 m2) (* 2 q2 m1)) 
(+ (* -2 q0 m2) (* -2 q1 m3) (* 2 q3 m1)) 
(+ (* 2q1 n1) (* 2q2 n2) (* 2 q3 n3)) 
(+ (* 2 q0 n3) (* -2 q1 n2) (* 2 q2 n1)) 
(+ (* -2 q0 n2) (* -2 q1 n3) (* 2 q3 n1))))) 


(defun partial-derivative-q2 (quaternion vector) 
(let ((q quaternion) 
(q-inv (quaternion-inverse quaternion)) 
(partial-q2 '(0 0 1 0)) 
(partial-q2-inv '(0 0 -1 0)) 
(v vector)) 
(vector-add (quaternion-product partial-q2-inv (quaternion-product v q)) 
(quaternion-product q-inv (quaternion-product v partial-q2))))) 


; alternative method 
(defun partial-derivative-q2-2 (quaternion vector) 
(let ((qO (first quaternion)) 
(ql (second quaternion)) : ut. 
(q2 (third quaternion)) | 
(q3 (fourth quaternion)) 
(m1 (first vector)) 
(m2 (second vector)) 
(m3 (third vector)) 
(n1 (fourth vector)) 
(n2 (fifth vector)) 
(n3 (sixth vector))) 
(list (+ (* -2 q0 m3) (* 2 q1 m2) (* -2 q2 m1)) 
(+ (* 2 q1 mil) (* 2 q2 m2) (* 2 q3 m3)) 
(+ (* 2 q0 m1) (* -2 q2 m3) (* 2 q3 m2)) 
(+ (ቾ -2 ባ0 83) (ቾ 2 ባ1 n2) (* -2 q2 n1)) 
(+ (* 2 ባ1 n1) (* 2 q2 n2) (* 2 q3 n3)) 
(+ (* 2 q0 n1) (* -2 q2 n3) (* 2 q3 n2))))) 


(defun partial-derivative-q3 (quaternion vector) 
(let ((q quaternion) 
(q-inv (quaternion-inverse quaternion)) 
(partial-q3 '(0 0 0 1)) 
(partial-q3-inv '(0 0 0 -1)) 
(v vector)) 
(vector-add (quaternion-product partial-q3-inv (quaternion-product v q)) 
(quaternion-product q-inv (quaternion-product v partial-q3))))) 


; alternative method 
(defun partial-derivative-q3-2 (quaternion vector) 
(let ((qO (first quaternion)) 
(ql (second quaternion)) 
(q2 (third quaternion)) 
(q3 (fourth quaternion)) 
(m1 (first vector)) 
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(m2 (second vector)) 
(m3 (third vector)) 
(nl (fourth vector)) 
(n2 (fifth vector)) 
(n3 (sixth vector))) 

(list (+ (* 2 q0 m2) (* 2 q1 m3) (* -2 q3 m1)) 
(+ (* -2 q0 m1) (* 2 q2 m3) (* -2 q3 m2)) 
(+ (* 2 ql m1) (* 2 q2 m2) (* 2 q3 m3)) 
(+ (* 2 q0 n2) (* 2 q1 n3) (* -2 q3 n1)) 
(+ (* -2 q0 n1) (* 2 q2 n3) (* -2 q3 n2)) 
(* (* 2q1 n1) (* 2 q2 n2) (* 2 q3 n3))))) 
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File: quaternion-algebra.lsp 


(defun quaternion-product (Q Q1) 
(let ((w (first Q)) (x (second Q)) (y (third Q)) (z (fourth Q)) 
(wl (first Q1)) (x1 (second Q1)) (y1 (third Q1)) (z1 (fourth Q1))) 
(list (- (* w w1) (*xxl)(*yyl)(*zzl) 
(+ (* x wl) (* w x1) (E z y1)) (* y 21)) 
(+ (* y w1) (*zx1) (* w yl) (-(*x z1))) 
(+ (* zwl) C (* y x1) (* x y1) (* w z1)))) 


(defun quaternion-inverse (Q) 


(list (first Q) (- (second Q)) (- (third Q)) (- (fourth Q)))) 


(defun rotate-vector (unit-quaternion vector) ; Vector is quaternion with leading 
(let* ((q unit-quaternion) (v vector) ;element zero. 
(q-inv (quaternion-inverse q))) 
(quaternion-product q (quaternion-product v q-inv)))) 


File: euler-to-quat.lsp 


(defun equivalent-quaternion (azimuth elevation roll) 
(quaternion-product (set-quaternion-azimuth azimuth) 
(quaternion-product (set-quaternion-elevation elevation) 
(set-quaternion-roll roll)))) 


(defun set-quaternion-azimuth (angle) 
(list (cos (/ angle 2)) 0 O (sin (/ angle 2)))) 


(defun set-quaternion-elevation (angle) 
(list (cos (/ angle 2)) 0 (sin (/ angle 2)) 0)) 


(defun set-quaternion-roll (angle) 
(list (cos (/ angle 2)) (sin (/ angle 2)) 0 0)) 


File: support-functions.lsp 
(defun deg-to-rad (angle) (* 0.017453292519943295 angle)) 
(defun firstn (n list) 
(cond ((zerop n) nil) 
(t (cons (first list) (firstn (1- n) (rest list)))))) 
(defun lastn (n list) 


(cond ((zerop n) nil) 
(t (append (lastn (1- n) (firstn (1- (length list)) list)) (last list))))) 
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; this function not required for Allegro CL for Windows 95 
(defun square (value) 
; (* value value)) 


File: vector-matrix-arithmetic.isp 


(defun augment (matrix) 
(concat-matrix matrix (unit-matrix (length matrix)))) 


(defun concat-matrix (A B) ;A and B are matrices with equal number of rows. 
(cond ((null A) B) 
(t (cons (append (car A) (car B)) (concat-matrix (cdr A) (cdr B)))))) 


(defun cycle-left (matrix) (mapcar 'row-cycle-left matrix)) 
(defun cycle-up (matrix) (append (cdr matrix) (list (car matrix)))) 


(defun dot-product (vector-1 vector-2) 
(apply '+ (mapcar '* vector-1 vector-2))) 


(defun first-square (matrix) ;Returns leftmost square matrix from argument. 
(do ((size (length matrix)) 
(remainder matrix (rest remainder)) 
(answer nil (cons (firstn size (first remainder)) answer))) 
((null remainder) (reverse answer)))) 


(defun matrix-inverse (M) 
(do ((M1 (max-car-first (augment M)) 
(cond ((null M1) nil) ;Abort for singular matrix. 
(t (max-car-firstn n (cycle-left (cycle-up M1)))))) 
(n (1- dength M)) (1- n))) 
((or (minusp n) (null M1)) (cond ((null M1) nil) (t (first-square M1)))) 
(setq M1 (cond ((zerop (caar M1)) nil) (t (solve-first-column M1)))))) 


(defun matrix-multiply (matrix1 matrix2) 
(cond ((null (rest matrix1)) (list (pre-multiply (first matrix1) matrix2))) 
(t (cons (pre-multiply (first matrix1) matrix2) 
(matrix-multiply (rest matrix1) matrix2))))) 
(defun max-car-first (L) ;L is a list of lists. This function finds list with 
(cond ((null (cdr L)) L) ;largest car and moves it to head of list of lists. 
(t (if (> (abs (caar L)) (abs (caar (max-car-first (cdr L))))) L 
(append (max-car-first (cdr L)) (list (car L))))))) 


(defun max-car-firstn (n list) 
(append (max-car-first (firstn n list)) (nthcdr n list))) 


(defun normalize-row (row) (scalar-multiply-vector (/ 1.0 (car row)) row)) 
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(defun normalize-vector (vector) 
(scalar-multiply-vector (/ 1 (norm vector)) vector)) 


(defun norm (vector) 
(sqrt (apply #+ (mapcar 'square vector)))) 


(defun post-multiply (matrix vector) 
(cond ((null (rest matrix)) (list (dot-product (first matrix) vector))) 
(t (cons (dot-product (first matrix) vector) 
(post-multiply (rest matrix) vector))))) 


(defun pre-multiply (vector matrix) 
(post-multiply (transpose matrix) vector)) 


(defun row-cycle-left (row) (append (cdr row) (list (car row)))) 


(defun scalar-multiply-vector (scalar vector) 
(cond ((null vector) nil) 
(t (cons (* scalar (first vector)) 
(scalar-multiply-vector scalar (rest vector)))))) 


(defun scalar-multiply-matrix (scalar matrix) 
(if (not (null matrix)) 
(cons (scalar-multiply-vector scalar (first matrix)) 
(scalar-multiply-matrix scalar (rest matrix))))) 


(defun solve-first-column (matrix) ‘Reduces first column to (10... 0). 
(do* ((remaining-row-list matrix (rest remaining-row-list)) 
(first-row (normalize-row (first matrix))) 
(answer (list first-row) 
(cons (vector-add (first remaining-row-list) 
(scalar-multiply-vector (- (caar remaining-row-list)) 
first-row)) 
answer))) 
((null (rest remaining-row-list)) (reverse answer)))) 


(defun transpose (matrix) ‚A matrix ıs a list of row vectors. 
(cond ((null (cdr matrix)) (mapcar ‘list (car matrix))) 
(t (mapcar 'cons (car matrix) (transpose (cdr matrix)))))) 


(defun unit-vector (one-column length) ¿Column count starts at 1. 
(do ((n length (1- n)) 
(vector nil (cons (cond ((= one-column n) 1) (t 0)) vector))) 
((zerop n) vector))) 


(defun unit-matrix (size) 
(do ((row-number size (1- row-number)) 
(I nil (Cons (unit-vector row-number size) I))) 
((zerop row-number) I))) 
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(defun vector-add (vector-1 vector-2) (mapcar ‘+ vector-1 vector-2)) 


(defun vector-subtract (vector-1 vector-2) (mapcar '- vector-1 vector-2)) 
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APPENDIX D. SIMULATION MODEL TEST RUNS USING 
GRADIENT DESCENT METHOD 


; (test-filter k offset max-iterations error delta-t) 
(test-filter-gradient .7 10-degrees 100 .001 .1) 


(-0.0407968594779338 0.987814785044294 0.149793422179633 -0.01093 14855508384) 
(-0.0359642597292573 0.989548859847923 0.139308154984263 -0.00963659435083765) 
(-0.0333988632303714 0.99 1038020142009 0.129027401800444 -0.00894919843069551) 
(-0.0309292427815184 0.992317459265856 0.119502133414506 -0.0082874656258 1398) 
(-0.028640593210504 0.993416060803329 0.110658722418457 -0.00767422382150284) 
(-0.0265169460797078 0.994358901419598 0.10245361366174 -0.00710519428779732) 
(-0.0245474425832684 0.995167708988955 0.0948440349768008 -0.00657746741643614) 
(-0.0227215761042829 0.995861275313425 0.0877894286954169 -0.00608822796790491) 
(-0.0210294198307998 0.996455827433058 0.0812514389056478 -0.00563481606095787) 
(-0.0194616178249921 0.996965358929859 0.0751939171141162 -0.00521472477960978) 
(-0.0180093776235814 0.997401923540244 0.06958289 18480387 -0.004825598 19042578) 
(-0.0166644560347113 0.997775893916602 0.0643 865 138599456 -0.00446522753680484) 
(-0.015419 1404136762 0.998096188622346 0.0595749837789895 -0.00413154622182663) 
(-0.014266226825664 0.998370470494064 0.055 1204677384272 -0.0038226239569759) 
(-0.0131989962170749 0.998605319423938 0.0509970053086573 -0.00353666037726664) 
(-0.012211189464283 0.998806382453515 0.0471804130930396 -0.00327197835557808) 
(-0.0112969819654345 0.9989785038606 0.0436481865581401 -0.00302701719454714) 
(-0.010450958277847 0.999125837688725 0.0403794020401677 -0.00280032583068047) 
(-0.00966808717378177 0.999251944930007 0.0373546203678796 -0.00259055615056851) 


; (test-filter k offset max-iterations error delta-t) 
(test-filter-gradient .8 10-degrees 100 .001 .1) 


(-0.0466363024932462 0.988054619864193 0.146345377276698 -0.0124961595910389) 
(-0.0336448060715241 0.990016369242001 0.13658090515966 -0.0090150986163666) 
(-0.0325765093727223 0.991635692032407 0.124584238621996 -0.00872884937864585) 
0.0294805 106677434 0.992994225320679 0.114153271356649 -0.00789927902587894) 
0.02705049608 76294 0.994134027077225 0.104466602633617 -0.00724815858154155) 
0.0247418690563448 0.995089762769567 0.0956046694999285 -0.00662956383288417) 
0.0226411424543706 0.99589074670975 0.0874768239624286 -0.0060666758363666) 
0.02071326208 17334 0.996561746884026 0.080030255471763 -0.00555010184741466) 
0.0189480025981257 0.997123653967 104 0.07320939 10107909 -0.00507710199435059) 
0.01733 14839560756 0.997594062141267 0.066963725303663 -0.004643957 1296634) 
0.0158516748037761 0.99798777087449 0.0612461657 160251 -0.00424744346235262) 
0.0144972763022684 0.998317215928941 0.0560131748731002 -0.00388453347764369) 
0.01325788503816 0.998592837826888 0.0512245341425605 -0.00355243958931966) 
0.0121239028963317 0.99882339562866 0.046843 1637111396 -0.00324858999018542) 
-0.01108649410346 0.999016233289641 0.0428349239064057 -0.00297061714191452) 
i .0101375328618482 0.999177505170899 0.0391684192251775 -0.00271634374357618) 


(- 
G 
(- 
(- 
e 
(- 
(- 
(- 
በ 
E 
G 
c 
(-0.00926955382915876 0.999312366554594 0.0358148057669039 -0.00248376946271993) 
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; (test-filter k offset max-iterations error delta-t) 
(test-filter-gradient .9 10-degrees 100 .001 .1) 


(-0.0524760351956661 0.988246613548474 0.142888723539894 -0.0140609112526659) 
(-0.0294523105119354 0.99045708628517 0.134406258574449 -0.00789172281690377) 
(-0.0328759984859507 0.992198765059205 0.11992989262179 -0.00880909724467732) 
(-0.0276080858352448 0.993618707837281 0.109109729514156 -0.00739756430412296) 
(-0.0257098919446979 0.994781415767648 0.0984960848930969 -0.00688894478407322) 
(-0.022999109006359 0.995733538414442 0.0891503444354995 -0.00616259268488926) 
(-0.0208838215796623 0.996512849414438 0.0805896639495956 -0.00559580312714617) 
(-0.0188515043342893 0.997150421192263 0.0728707295195703 -0.00505124536248464) 
(-0.0170522400191014 0.99767183 1074489 0.06587307 19325037 -0.004569 13394225991) 
(-0.0154104027156919 0.998098107382105 0.0595452533353919 -0.00412920496270799) 
(-0.0139299632575643 0.9984465 16862027 0.0538198603326016 -0.0037325224054596) 
(-0.0125894270161801 0.998731222574884 0.0486422866214839 -0.00337332680215604) 
(-0.011377821997732 0.99896383 1698321 0.0439603649476757 -0.00304867821591733) 
(-0.0102822040612832 0.999153850020246 0.0397274419986532 -0.00275510827463285) 
(-0.00929 1825 16300677 0.99930905 7873637 0.0359008362893083 -0.00248973704863885) 


; (test-filter k offset max-iterations error delta-t) 
(test-filter-gradient 1.0 10-degrees 100 .001 .1) 


(-0.0583152058497816 0.9883906822829 16 0.139423955287233 -0.0156255 123139037) 
(-0.0233841113470548 0.99084953 1450444 0.13278226421806 -0.00626575375116281) 
(-0.035 1423427807272 0.992718898589437 0.114828290272939 -0.00941636236823354) 
(-0.0244014962468546 0.994191294500872 0.104620680327731 -0.00653836121345577) 
(-0.0252759303508672 0.9953635 13062414 0.0925566594905315 -0.006772665 12546018) 
(-0.0208793028447936 0.996299393515018 0.0831881826398405 -0.00559459233578731) 
(-0.0194982062305918 0.997046937933426 0.0740940470762447 -0.0052245286 1334252) 
(-0.0170032438817054 0.997643944329682 0.0663075625701921 -0.00455600546681241) 
(-0.0153929960071091 0.9981205821668 0.0591721834286807 -0.00412454084920038) 
(-0.0136473996816812 0.9985010080441 0.0528782862760464 -0.00365680972349124) 
(-0.0122381790122951 0.998804566699065 0.0472113471120067 -0.00327921018317201) 
(-0.0109045963485031 0.999046739936555 0.0421682799130494 -0.00292187778536873) 
(-0.00974982771925494 0.999239909185641 0.0376526202393971 -0.00261245846371697) 
(-0.00870022237272814 0.999393970093261 0.0336238620118091 -0.00233121755874369) 


; (test-filter K offset max-iterations error delta-t) 
(test-filter-gradient 1.1 10-degrees 100 .001 .1) 


(-0.064152962096264 0.988486763117717 0.135951570888611 -0.0171897343857584) 
(-0.0154383501349454 0.991160049344576 0.131705359403706 -0.00413669345112755) 
(-0.0402208972792998 0.993164779155011 0.109040605997087 -0.0107771569448435) 
(-0.0184433682027599 0.994695655034773 0.10107410049485 -0.00494188561563934) 
0.0272749736590781 0.99587624841727 0.086215789 1375802 -0.00730830716553013) 
0.0170203420463652 0.996791380025529 0.078079790666324 -0.00456058690622508) 
0.0195615695694212 0.997502548657585 0.0676648876773959 -0.00524150676881165) 
0.0143499320307392 0.998055873164189 0.0605290761776836 -0.00384505269907804) 
0.0145360733162948 0.99848660371862 0.0528964487868246 -0.00389492910622076) 
-0.0116238695093196 0.998821956912581 0.0470093955888315 -0.0031 1460644794699) 
0.011028608011913 0.99908305378144 0.0412636498263716 -0.00295510661043149) 

0. 


(- 
(- 
t 
(S 
(- 
à 
(-0.00923422502702625 0.999286324444643 0.0365437914021331 -0.00247430313871896) 


84 


; (test-filter K offset max-iterations error delta-t) 
(test-filter-gradient 1.2 10-degrees 100 .001 .1 


(-0.0699884515727754 0.988534814043835 0.132472072408145 -0.0187533490784299) 
(-0.00561498066974421 0.99134280914623 0.131170282979858 -0.00150452953597429) 
(-0.0489546216571366 0.993459214850227 0.102323841510331 -0.0131173513388009) 
(-0.00780925307707703 0.995055303108517 0.0989928322489017 -0.00209248305549306) 
(-0.0344797715312819 0.996259806578048 0.0786904519560616 -0.00923882693701665) 
(-0.00815 760284656923 0.99716942915779 0.0747114800169442 -0.00218582309491202) 
(-0.024468423836436 0.997856825885085 0.0603329617468896 -0.00655629440703547) 
(-0.00759078677198126 0.998376615370229 0.0564125598165019 -0.0020339451854692) 
(-0.01749636685 17397 0.998769890812519 0.046158463 1606423 -0.004688 13736840233) 
(-0.00663835602497839 0.999067597782413 0.0426228033683549 -0.00177874213596324) 
(-0.0126030030050699 0.999293061991474 0.0352581434831337 -0.0033769644774155) 
(-0.00558803870908517 0.999463881528935 0.032225477558732 -0.00149731045937329) 
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